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ABSTRACT 


The  estimation  of  launch  vehicle  performance  parameters 
was  explored  through  the  use  of  a  Bayes  filter.  The  main 
emphasis  was  to  nse  an  eight  state  model  that  would  include 
the  vehicle  position  and  velocity  vectors,  the  vehicle 
exhaust  velocity,  and  the  ratio  of  the  mass  flow  rate  to  the 
initial  mass.  A  primary  objective  was  to  be  able  to  observe 
these  quantities  through  the  staging  events,  where  the  last 
two  elements  of  the  state  would  be  changing  very  rapidly.  The 
results  indicated  that  indeed  the  staging  event  was 
observable.  However,  as  would  be  expected,  the  data  processed 
at  the  exact  time  of  staging  included  errors  which  diminished 


as  the  filter  processed  more  data.  A  fading  memory  was  added 


in  an  attempt  to  improve  the  filters  performance  in  the  area 
of  the  staging  event.  This  proved  to  be  marginally  successful 
as  several  Bayes  loop  iterations  had  to  be  performed  to 
notice  the  effect  of  the  fading  memory  addition.  Care  was 
taken  to  show  each  step  of  the  filter  development  and  its 
checkout.  Several  numerical  tables  are  presented  including 


the  input  and  output  data. 


d  //  >  '■ 


I  INTRODUCTION 


The  specific  problem  that  will  be  examined  in  this  paper 
is  the  estimation  of  launch  vehicle  performance  parameters 
from  either  a  ground  based  or  a  space  based  sensor  system. 
We  can  easily  understand  the  importance  of  this  since,  from  a 
military  point  of  view,  it  is  a  necessity  that  the  United 
States  be  able  to  obtain  the  launch  vehicle  characteristics 
of  the  weapons  of  hostile  countries.  This  is  essential  in 
the  planning  of  our  strategic  policies  and  is  needed  to 
determine  where  the  emphasis  should  be  placed  in  trying  to 
develop  or  improve  existing  U.S.  systems.  The  individual 
parameters  must  be  known  to  some  tolerance  so  an  adequate 
comparison  can  be  made  with  respect  to  the  U.S.  systems. 
Since  these  countries  do  not  publish  any  data  relating  to 
their  launch  vehicle  systems,  we  must  rely  on  estimation 
systems  which  use  observation  data  to  determine  these 
parameters.  The  data  can  take  many  forms  since  it  can  be 
supplied  from  radar  systems  that  are  ground  or  space-based, 
or  any  other  form  of  detection.  However  in  all  cases,  the 
basic  form  of  the  estimator  is  essentially  the  same. 

The  problem  scenario  is  developed  as  follows.  Upon  the 
launch  of  an  ICBM  from  a  hostile  country,  our  sensor  systems 
respond  by  first  detecting  that  a  launch  has  indeed  occured, 
and  the  subsequent  tracking  procedure  is  then  carried  out. 
The  utility  of  the  estimator,  the  development  of  which  will 


be  examined  in  this  paper,  is  to  take  this  subsequent 
tracking  data  and  estimate  the  vehicle  performance 
parameters.  Upon  successful  target  recognition,  data 
transmittal,  and  estimation  calculation,  some  of  the  ICBM's 
performance  characteristics  will  be  known. 

The  following  diagram  shows  the  general  geometry  for  an 
orbiting  sensor. 


Figure  1  Orbital  Sensor  Geometry 

Note  that  the  initial  conditions,  to  be  discussed  later, 
will  be  the  launch  site  of  the  I CBM,  either  on  the  ground  or 


in  the  silo.  Recognizing  the  physical  limitations  in 
acquiring  the  target,  some  time  will  elapse  before  the 
tracking  procedure  is  fully  functioning,  and  the  trajectory 
portion  that  is  actually  observed  will  be  only  a  subset  of 
the  entire  flight. 

The  situation  with  a  land  based  sensor  is  very  similar, 
with  a  few  distinctions.  The  basic  situation  is  shown  below. 


Added  difficulty  appears  in  target  acquistion  since, 
although  the  site  may  be  closer,  instead  of  looking  down  on 
the  target  launch,  the  sensor  is  forced  to  discriminate  as 
the  vehicle  rises  over  the  horizion.  In  addition,  depending 
on  the  launch  site,  the  vehicle  may  not  be  in  view  until  a 
significant  part  of  it's  trajectory  has  been  flown.  This 
would  only  serve  to  complicate  the  tracking  procedure,  and 


3 


wh  ere 


x,  y,  z  are  the  3  components  of  position 
i,  f,  i  are  the  3  components  of  velocity 
Ve  is  the  exhaust  velocity 
M  is  the  ratio  m  over  initial  mass 

The  two-body  equation  is  non-linear,  so  to  move  the 
state  through  time,  it  is  differentiated  and  written  in  a 
general  form  of  the  equation  of  motion  as  : 


d/dt  ( x ( t ) )  =  F  (x(t).t)  (3-2) 

where  x(t)  is  the  state  vector  at  each  time. 

Notice  that  this  is  simply  a  different  expression  for 
equation  2-5. 

Using  the  equations  of  motion  that  were  developed  in 
the  last  section,  the  F  vector  is  found  as  follows: 
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where  a  = 


Note  here  that  the  Ve  and  the  M  are  assumed  to  be  constant. 
In  this  program,  variations  in  exhaust  velocity  and  mass 


ratio  are  handled  through  the  white  Gaussian  noise  of  the 


Ill  FILTER  DEVELOPMENT 


Matrix  Equations 

The  filter  will  be  receiving  sampled  input  data  from 
the  sensors.  In  order  to  process  this  data,  a  Bayes  filter 
estimator  was  used.  The  Bayes  filter  was  c?-osen  since  the 
problem  dynamics  are  basically  deterministic.  Equations  of 
motion  can  be  written  specifically  for  the  vehicle,  and  since 
it  is  desired  to  evaluate  each  stage's  performance,  several 
data  segments  will  need  to  be  processed  to  obtain  the 
results.  The  bayes  filter  uses  sequential  data  segments  and 
produces  cnrrent  estimates  of  the  state  and  covariance, 
thereby  lending  itself  to  the  specific  problem  of  observing 
each  of  the  stage's  performance. 

To  start,  define  the  state  vector  (X)  describing  the 
state  of  the  vehicle.  Recalling  the  launch  vehicle  equations 


of  motion: 


The  partial  of  rangs,  azimuth,  and  elevation  then  becomes  an 
identity  matrix  since  the  data  matrix  (6)  contains  only  the 
variables  (range,  azimuth,  elevation)  and  no  functions  of 
these  quantities.  The  'noisy*  data  is  then  formed  as: 


range 

az imuth  = 
el eva  tion 

no  i  sy 


range 
az imuth 
elevation 


/ 


range 


perfect 


61  az imuth  1 
\el  ovation/ 


(2-23) 


The  next  task  is  to  develop  the  filter  which  will 


implement  the  dynamics  formulated  in  this  chapter. 


A  range  of  vehicles  was  chosen  to  ensure  that  the  filter 
wonld  operate,  given  a  wide  variety  of  possible  input 
traj  ectories. 


Runs  made  with  the  previous  information  produced  actual 
results.  Next  the  programs  in  Appendix  A  were  run  containing 
the  option  to  calculate  noisy  data  so  that  the  program  could 
simulate  incorrect  measurements  from  the  sensors.  The  result 
here  was  to  have  the  data  files  written  with  the  random 
errors  included  in  each  measuerment  of  range,  azimuth  and 
elevation.  Basically  the  approach  taken  was  to  assume  that 
the  errors  would  occur  randomly  as  per  a  Gaussian 
distribution.  To  obtain  the  difference  between  measurements 
with  and  without  the  noise  included  (designated  d(  )  ),  let: 


range 
az imuth 
elevation 


range 
az imuth 
el evation 


(2-20) 


where 


G|t  represents  a  gaussian  function  whose  mean  =  0 

and  standard  deviation  is  ±  1 
o(range,  azimuth,  e  1  ev a t i o n )  i s t h e  defined  accuracy  of 

each  measurement 

The  <r(range,  azimuth,  elevation)  accuracies  were  input 
as  follows: 


Table  1.  Launch  Vehicle  data 


Quantity 

St  g  I 

St  g  II 

St  g  III 

St g  IV 

Titan  IIIB 

I,_  sec 

256 

317 

292  (Agena) 

.‘p  n 

305970 

73  816 

14676 

F  lb 

434900 

102300 

16000 

Titan  HID 

ISp  sec 
m0  lb 

301 

317 

444 

284 

307500 

73670 

36122 

2721 

F  lb 

523000 

102300 

30000 

15000 

Thor  LV-2F 

rsp  sec 
m0  lb 

251 

290 

106092 

1743.7 

F  lb 

170000 

10000 

Using  equation  (2-19)  the  parameters  are  calculated  which  are 
needed  to  enable  the  correct  calculation  of  the  A  matrix. 
The  A  matrix  is  simply  the  assemblage  of  the  equations  of 
variation.  The  values  are  as  follows. 


Table  2.  Launch  Vehicle  Performance  Parameters 


Stg 

Quantity 

TitanHIB 

TitanHID 

Thor LV- 2F 

mm 

V. 

.317298 

.  3736  93 

.311618 

mm 

M 

4.479637 

4 .5  51364 

5 .14213  8 

Ve 

.3  93557 

.393557 

. 360036 

'■1 

M 

3 .521417 

3  .5283  96 

15 .928772 

H 

ve 

. 362519 

.551228 

M 

3 .007333 

1.506670 

IV 

ve 

.352587 

M 

15.634947 

where 


Ve  is  in  DU/TU 
M  is  non  dimensional 


Then,  recalling  Figure  4,  the  data  is  assembled  as: 
range  =  p 

azimuth  =  tan-*  (  ^  /  x  )  (2-18) 

elevation  =  tan-*  (  56  /  "\J x2  +  y2  * 

This  result  is  general  in  that  the  mechanization  of 
finding  the  unit  vectors  SEZ  is  the  same  for  both  the  radar 
site  and  the  orbiting  sensor.  The  only  difference  is  the 
initial  calculation  of  rs» 

Truth  Model  Development 

Programming  the  equations  of  motion  and  numerically 
integrating  them  provide  the  numerical  integration  and  truth 
model  data.  Appendix  A  lists  the  programs  which  were  used  to 
generate  the  data.  In  order  to  look  at  the  launch  vehicle  in 
particular  however,  something  must  be  known  about  their 
characteristics,  specifically,  the  exhaust  velocity  V@,  and 
the  mass  ratio,  M  =  m  over  initial  mass.  This  data  was 
obtained  for  a  few  different  missiles  from  the  D. S.  Space 
Launch  Systems  document,  published  by  the  Navy,  Reference  5. 
Recalling  from  basic  propulsion  (Sutton  Reference  4)  that: 

ih  =  F/Ve  and  V  e  =  Isp  g  and  M  =  “/ ^  (2-1R) 

only  I,p>  m0>  and  F  need  to  be  obtained.  The  values  are  as 


f  ol 1 ow  s 


Figure  5.  Observation  Geometry 

Z  =  ?s  /  |^|  (2-11) 

the  east  unit  vector  then  is 


A 

E 

A 

k 
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i  A 

Ik  x  : 
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(2-12) 
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A 

E 
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(2-13) 

transformation  matrix  is 

then 

formed 

as 

'  I  ‘ 

J 

.  K  . 

= 

S 

1  A 

1  E 

1 

1  z  1 

1  J 

'  S  ‘ 
E 

z  . 

(2-14) 

Recalling  that  the  inverse  of  an  orthogonal  basis  vector  is 
the  sane  as  the  transpose,  this  is  also: 


A 

1  _  , 

'  S 

I 

E 

= 

— i — 

J 

.z. 

z 

.  K  . 

To  complete  the  process  of  finding  range,  azimuth  and 
elevation,  the  launch  vehicle's  position  vector  is  computed 
a  s  : 

pJJK  =  r  -  fs  (IJK)  (2-16) 

transforming  to  SEZ: 


11 


resents  the  r$  of  the  orbitin 


ector  for  both  cases,  calculat 


The  coordinate  system  for  a  land  based  radar  site  is  shown  in 


Figure  3.  Notice  that  given  only  latitude,  local  sidereal 
time  and  elevation,  the  site  position  vector  is  obtained. 


Figure  3.  Radar  Site  Coordinate  System 


Looking  now  at  an  orbiting  sensor,  the  orbit  of  the 
sensor  mast  be  known.  The  5  classical  orbit  elements  are 
input  (Reference  1),  and  position  and  velocity  vectors  can  be 
calculated.  From  these,  the  mean  motion  is  calculated  as: 

n  (2_9) 

where 

n  =  mean  motion 
a  =  semi  major  axis 

The  mean  anomaly  M  is  then  calculated  as: 

M  =  n  (  t  -  t0  )  (2-10) 

where  (t  -  tQ)  =  the  time  in  dayspastthe  initial  time 


Position  and  velocity  vectors  can  then  be  calculated.  (See 
Appendix  A  for  a  more  complete  description  of  the  r  and  v 


examined  an  orbiting  sensor  looking  down  upon  the  ICBM 
trajectory.  The  observation  relationships  can  be  considered 
identical  once  the  position  vector  of  the  observer  (radar 
site,  or  orbiting  sensor)  are  known. 

For  a  radar  site,  the  position  vector  can  be  determined 
given  latitnde,  longitnde,  elevation,  and  universal  time. 
The  first  step  is  to  calculate  the  local  sidereal  time  for 
the  site. 

0  =  0g  +  (2-6) 

where 

6  =  Local  sidereal  time 

Og  =  greenwich  sidereal  time 
Xe  =  longitude  of  the  site 

The  Greenwich  Sidereal  Time  is  then  calculated  as: 

©g  =  0go  +  1.0027379093  (  t  -  tQ  )2t  (2-7) 

where 

=  Greenwich  Sidereal  Time  (deg) 

=  the  value  in  deg  on  1  Jan  1984,  (98.85481') 
t0)=  the  time  in  days  past  the  initial  time 

The  position  vector  of  the  site  can  then  be  calculated  as: 

cos  (  L  )  cos  (6) 

cos  (  L  ) sin  (6)  (2—8) 

sin  (  L  ) 

where 

h  =  the  distance  from  center  of  the  earth  to  the  site 

r s  =  the  site  vector 

L  =  latitude  of  the  site 

6  =  local  sidereal  time 


that  would  be  considered. 

The  vehicle  will  also  undergo  an  acceleration  due  to 
thrusting  during  the  propulsive  phase  o£  the  flight  which  is 
equal  to: 


a 


v  _ SL, — 

e  m0  -  m  t 


(2-3) 


where 

a  =  vehicle  acceleration  due  to  thrust 
m  =  mass  flow  rate 
m0  =  initial  Bass 
t  =  time 

Ve  =  Vehicle  exhaust  velocity 


But  since  absolute  masses  are  not  observable  from  the 

trajectory  data,  let  M  =  “/  ,  and: 

*0 

I  -  v„  At  <*-«> 


To  obtain  the  total  vehicle  acceleration,  equations  (2-1)  and 
(2-4)  are  added  to  get: 


i  =  -  f */t3  *  Ve  r^STt  (2-5) 

where  the  r  denotes  the  total  vehicle  acceleration. 


These  equations  constitute  the  equations  of  motion  for 
the  launch  vehicle  when  they  are  numerically  integrated.  The 
next  task  is  to  develop  the  observation  relationships. 

Observation  Relationships 

Two  cases  were  considered  for  the  observer.  The  first 
case  considered  a  radar  site  observing  the  trajectory  of  an 


ICBM  as  it  could  be  seen  above  the  horizon.  The  second  case 


II  DYNAMICS  FORMULATION 


( 


( 


< 


Equations  of  Motion 

For  the  specific  problem,  the  equations  of  motion  for 
the  trajectory  of  the  launch  vehicle  must  be  generated. 
Numerically  integrating  these  equations  on  the  computer,  will 
simulate  the  data  that  the  radar  sites  would  be  observing 
and  providing  to  the  sensor  system. 

The  underlying  assumption  of  this  work  is  the  spherical 
earth  model  and  the  use  of  the  two  body  equation  of  motion 
(Reference  1),  and  Newton's  Law  that  the  mass  times  the 
acceleration  is  equal  to  the  sum  of  the  forces.  In  general, 
the  two  body  equation  of  motion  is  written  as: 

i  +  f  *‘/r3  =  0  (2-1) 

where 

f  =  vehicle  acceleration 

r  =  radius  vector  from  center  of  Earth  to  vehicle 

r  =  radius  vector  magnitude 

|i  =  gravitational  parameter  defined  by: 

p  =  GM  (2-2) 

where 

G  =  Gr av i ta tr i onal  potential 
M  =  Mass  of  the  Earth 

Notice  that  only  gravitational  forces  are  considered  since  in 
this  paper,  all  other  external  forces  are  assumed  to  be  zero. 
The  assumption  is  made  since  the  acceleration  due  to  thrust 
is  several  orders  of  magnitude  larger  than  the  other  forces 
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the  use  of  radar  data  in  conjunction  with  the  infared  data  ao 
that  range,  azimuth  and  elevation  measurements  will  be 
available  for  computation.  In  addition,  a  different  local 
coordinate  system  is  adopted  to  make  the  computations  easier 
in  the  derivation  of  the  various  observation  relationships. 
The  analysis  starts  with  a  specific  look  at  the  problem  and 
what  data  is  avialable.  The  dynamics  are  then  formulated, 
yielding  the  equations  of  motion  and  the  filter  algorithm 
that  will  produce  tangible  results.  In  order  to  assess  the 
results  of  the  filter  algorithm,  a  truth  model  was  developed 
which  simulated  the  data  for  each  parameter  that  was  modeled. 
Specifically,  a  reference  thrust  profile  was  developed 
through  the  computer  programs  contained  in  the  appendices, 
and  this  was  used  as  the  'true'  profile.  The  simulation  then 
proceeds  to  try  to  observe  this  quantity,  and  the  performance 
of  the  estimator  can  thus  be  observed. 


make  the  target  acquisition  process  slower. 

Previous  work  was  done  nsing  infared  sensors  to 
determine  the  position  and  velocity  vectors  and  vehicle 
acceleration  for  the  launch  vehicle  (  Reference  3  ).  The 
method  used  azimuth  and  elevation  measurements  and  concluded 
that  a  7-state  filter  could  solve  for  vehicle  acceleration. 
One  of  the  problems  with  this  particular  approach  was  the 
extreme  complexity  of  the  data  and  observation  relationships. 
This  made  for  extremely  difficult  checkout  and  computer 
coding.  Gross's  presentation  (Reference  2),  provided 
additional  work  centered  on  a  space-based  infared  sensor 
system  in  an  attempt  to  get  additional  data  from  the  orbiting 
sensor.  The  main  emphasis  here  centered  on  a  Bayes  filter  to 
observe  exhaust  velocity  and  vehicle  mass,  and  the  same 
filter  to  observe  vehicle  acceleration.  The  simulation  did 
not  yield  significant  results  for  the  first  case,  but  was 
able  to  observe  the  vehicle  acceleration.  The  main  result 
seemed  to  conclude  that  a  fading  memory  differential 
corrector  might  be  useful  and  that  additional  input  data 
could  change  the  poor  results  in  the  expanded  estimation 
system.  As  before  however,  the  observation  relationships 
were  extremely  involved. 

The  majority  of  this  paper  will  be  based  on  trying  to 
extend  the  Bayes  filter  analysis  already  started  in  an 
attempt  to  develop  a  filter  that  will  provide  as  much  data  as 
possible.  The  primary  difference  with  this  attempt  will  be 


observations.  The  dynamics  model  could  incorporate  very 
complex  expressions  to  represent  the  variations,  bnt  this  was 
not  done  in  this  paper. 

The  previous  relations  establish  the  laws  that  will 
govern  the  motion  of  the  launch  vehicle.  The  next  problem  is 
to  determine  how  to  correct  the  estimate  of  the  state  from 
the  input  data  (coming  from  the  radar  sites  or  satellites), 
which  the  estimator  will  be  proccessing.  To  accomplish  this, 
a  vector  is  found  through  the  observation  relationships  which 
were  developed  in  the  previous  chapter,  and  the  following 
operations  are  performed. 

In  order  to  estimate  the  state,  a  nominal  trajectory  is 
assumed  as  a  function  of  time(x(t)),  with  initial  conditions, 
and  the  true  trajectory  is  written  as: 

x(t)  =  x0(t)  +  6x(t)  (3-4) 

where 

x(t)  =  the  true  solution 

6x(t)  =the  difference  between  the  nominal  and  true 
tr aj  e  ct  ory 

Differentiating  yields: 

i(t)  =  i0(t)  +  8  x ( t )  (3-5) 

and  adding  to  Equation  (3-2): 

i0(t)  +  8x  (t)  =  F  ( xQ ( t  )  +  8x(t)  ,  t)  (3-6) 

To  solve  this  we  expand  the  right  hand  side  of  Equation  3-6 
with  Taylor's  theorem,  and  obtain: 


i0( t )  +  8  x ( t )  =  F(x0(t),t)  +  A(t)go(t)  8x(t) 

+  1/1 2  Vx  <  Vx8>  x0(t)  (6*(t))2  +  H.O.T.  (3-7) 

where  A(t)  =  dF/dx  (the  A  matrix  is  derived  in  Appendix  D) 

Now  assuming  that  Sx  is  sm al 1 ,  Equa t ion  (3-7)  becomes: 

£0(t)  =  F(x0(t).t)  (3-*) 

Ignoring  higher  order  terms  in  Equation  (3-7)  and  subtracting 
Equation  (3-8) ,  we  obtain: 

8x ( t )  =  A(t)j  {t)  8x ( t )  (3-9) 

Recalling  the  idea  of  state  transition  matrices,  the 
state  variations  are  moved  through  time  as  follows: 

6x(t)  =  f  (t,tc)  Sx  ( tQ)  (3-10) 

where  ^  (t.t0)  is  the  state  transition  matrix 
^  is  then  calculated  from: 

^  (t.tQ)  =  A(t)  io(t)  +  (t,tQ)  (3-11) 

The  state  transition  matrix  definition  also  prescribes  the 
initial  conditions  as: 

if  <t0.t0)  =  I  (3-12) 

The  preceding  equations  will  enable  the  calculation  of 
the  state  vector.  propagation  through  time,  and  the 
estimation  of  the  errors  from  the  true  trajectory  as  a 
function  of  time. 

The  next  part  of  the  problem  is  to  process  the  data 
that  will  be  coming  in  from  the  sensors.  The  predicted  data 
for  each  observation  is  given  by: 

z  (  t  ± )  =  G  (Ktj)  ,  tj)  (3-13) 
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Evaluating  this  at  the  initial  time,  the  initial  conditions 
become  : 


ijjttj)  =  G  ( x0  ( t  i )  ,  tj)  (3-14) 

Also,  knowing  that  there  will  be  some  difference  between 
this  and  the  true  measnrment,  let: 

z(t.)  =  G  +  8x(ti),ti)  (3-15) 

where  6x(t^)  is  the  difference  from  the  true  data 

This  equation  can  be  expanded  in  a  Taylor  series  as  follows: 

£(t.)  =  G(s0(ti),ti)  +  3G(xA(ti),tj)  Siltj) 

( t l )  XoCti) 

+  H.  0.  T.  (3-16) 

Subtracting  this  'true'  relation  from  the  calculated  relation 
gives  the  residual  of  the  observation: 

rUj)  =  z(ti)  -  z0(ti) 

=  dG/di  |xD  (t) 

=  H(i0(ti),ti)6x(ti)  (3-17) 

Note  that  here,  as  before,  the  higher  order  terms  have  been 
ignored. 

In  the  previous  chapters,  observation  relationships 
were  developed.  The  data  vector  G  consists  of  the  range, 
azimuth  and  elevation. 

range 

[G]  =  azimuth  (3-1$) 


The  H  matrix  (observation  relation)  is  simply  the 
partial  derivative  of  G  with  respect  to  the  state  vector,  so: 


[H]  =  dG  /  di  (3-19) 

Using  the  observation  relationships  that  were  developed  in 
the  last  chapters: 

range  ^ 

azimnth  =  tan-*  (  T  /  x  )  (3-20) 

elevation  tan-*  (  z  +  y^) 

Since  only  x,  y,  and  z  appear  in  the  G  matrix,  only  t: 
first  3x3  block  of  the  H  matrix  will  have  non  zero 
elements.  Using  the  following  definition  then: 

d/dx  tan'*  u  =  */1+u  (du/dx)  (3-21) 

The  first  3x3  of  the  H  matrix  is  then: 


(3-22) 


The  final  step  is  to  move  the  residuals  to  a  single 
epoch  time.  Using  the  state  transition  matrix  which  was 
developed  before: 


f(ti)  =  H(xc(ti)  . t£)  +  ( t  £ , 1 0 )  6x(t0) 
*  T(t.)  6x(t0) 


(3-23) 


With  the  residuals  calculated,  the  necessary  matrices  are 
available  and  the  filter  can  be  derived. 

Least  Squares  Filter  Development 

Summarizing  the  data  already  obtained,  the  state  vector 
is  given  by: 

i  =  F ( x  >  t)  (3-24) 

deviations  of  the  state  vector  are  known  as: 

6i(t)  =  t  (t.tQ)  5i(t0)  (3-25) 

The  observation  r e al t i on s h i p s  were  developed  as  G.  and  the 
residual  data  was: 

f(t.)  =  Ktj)  8  x  ( 1 0  )  ( 3-26) 

The  sensors  will  not  input  perfect  data,  and  the  covariance 
matrix  Q  tells  us  how  accurate  the  data  is  (for  each 
measurment  of  range,  azimuth  and  elevation),  and  how  each 
quantity  affects  the  other.  (See  chapter  2  for  numerical 
values.  )  The  residual  vector,  including  this  error  is 
written  as: 

i(t.)  =  T( t i )  6  x ( t  0 )  +  e(tj)  (3-27) 

To  calculate  this  error,  rearrange  as: 

e(tA)  =  r( t  i )  -  T(tt)  6x(t0)  (3-28) 

Assuming  that  for  each  observation,  a  random  errors  in  range, 
azimuth  or  elevation  are  uncorrelated  with  each  other,  a  data 
covariance  matrix  (Q)  defined  which  contains  the  information 
about  the  accuaracy  of  the  measurments.  Using  Gaussian  error 


statistics,  the  probability  density  function  for  the  error 
vector  e(t^)  is: 


P(e)  =  (2n)~N^2  [  Q  ]_1 eip(  J / 2)  (3-29) 


where 


N 

Q 

J 


number  of  seainreients 
data  covariance  matrix 

e^Q~^e  (a  scalar)  wieghted  least  squares  function 


Using  the  principle  of  maximum  likelihood,  J  is  minimized  to 


make  P  a  maximum.  (P  is  a  maximum  when  the  residual  errors  e 


are  the  smallest)  Thus: 


9  J  d _ 

d  x  -  3  x  (eTQ-1 e)  =  0 

Now  substituting  into  J  as: 

J  -  (r  -  T  Sx)T  Q-1  (r  -  T  6x) 

=  rTQ_1r  -  fTQ~1T6x-6xTTQ"1r  +  6xTTTQ_1T6x 
Note  here  that  the  functional  dependence  on 
intentionally  left  out  to  enhance  clarity.  Thus, 
(3-3  0)  be  come  s : 


(3-30) 

(3-31) 

time  is 
Equa t i on 


0  =  -(rTCT1T)T-TTQ-1f+(6xTTTQ_1T)T+TTQ_1T5x 

=  +  2TtQ_1T6x  (3-32) 

and  solving  for  Si: 

6x  <=  (TTQ-1T)_1  TTQ-1F  (3-33) 

This  result  is  valid  when  the  trajectories  are  very  close. 
Iteration  is  needed  to  get  the  trajectories  to  have  a  very 
small  difference. 

Next,  the  covariance  is  to  be  calculated.  In  general. 
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the  Sz  can  be  written  as: 


Sx  =  Wf  (3-34) 

where  W  =  (TTQ_1T)_1  TTQ_1 

The  covariance  of  z  which  prodnced  this  error,  given  x(t0) 
at  the  epoch  time  (assuming  Sz  as  zero  mean)  is: 

P5(tQ)  =  E(6x,SxT)  (3-35) 

where  Sz  is  Wr  =  z  -  zQ 

Substituting  and  recognizing  that  V  can  be  calculated  (the 
assumption  of  deterministic  dynamics  for  the  problem),  it  is 
pulled  out  of  the  expectation  operator: 

P£(t0)  =  W  E(ffT)WT  (3-36) 

But  recall  that  E(rrT)  is  defined  as  the  covariance  matrix  Q, 
where  r  is  the  zero  mean,  thus: 

Pi(tQ)  =  W  Q  W  T  (3-37) 

Expanding , 

P  ~ ( 1 0 )  =  (TTQ-1T)-1  tTq-1  Q[(TTQ_1T)_1  ttq-1]T 
=  (TTQ-1T)_1  ttq_1t  (TTQ-1T)-1 
=  (TtQ_1T)_1  (3-3*) 

The  final  step  which  is  needed  is  to  define  when  the 


estimator  has  reached  convergence.  Under  perfect  conditions, 
the  Sz  will  converge  to  zero.  However,  it  is  sufficient  to 


be  sunotrized  and  written  as  shown  in  Appendix  H. 


Bayes  Filter  Development 

The  process  of  changing  to  a  Bayes  filter  is  relatively 
easy  since  the  only  real  difference  is  that  the  Bayes  filter 
will  process  segments  of  data  in  a  least  squares  mode,  and 
then  update  it's  state  and  covariance  matrix  to  some  time  in 
the  future.  The  major  changes  are  detailed  below. 

Again  recalling  the  Least  Squares  development,  the 
matrices  that  will  change  are  as  follows,  realizing  that  the 
previous  estimate  and  matrices  can  now  be  treated  as  ’data': 


T  = 


I 

P(-)  |  0 

— 

Q 

= 

-  - - 

.  Tn 

.  °  1  «. 

f 

s 

*  w 

i(-)  -  £tef 

(3- 

Zn  -  G( x ) n 

where  the  subscript  n  denotes  the  new  portion  of  data 
Then : 


P'1(+)  =  (P"1(-)  TnTQn_1) 


P_1(-)  +  TnTQn_1Tn 


r  i 
I 


and  the  corrections  are: 

hi  ( t  )  =  P(  +  )TTCT1f 


(3-40) 


(3-41) 


P(+)  (P_1(-)  TnTn  ” 


and  finally: 


of----] 

L  *n 


-  P(  +  )  (P-1(-)  (£(-)-£_,)  +  T„TQ  r„ ) 


6X(t„) 


(3-42) 


One  additional  device  mast  be  added  to  the  least 
squares  developement  to  incorporate  the  fading  memory  effect. 
The  object  here  is  to  have  the  filter  retain  full  memory 
about  components  which  do  not  change  drastically  during  the 
flight,  (position  and  velocity  vectors)  and  to  retain  little 
memory  of  those  elements  which  will  change  rapidly  at 
certain  times,  (exhaust  velocity  and  mass  ratio).  The  staging 
event  is  the  primary  cause  of  the  changes.  The  fading  memory 
filtering  is  accomplished  by  multiplying  the  covariance 
matrix  by  a  matrix  of  scalar  0's  after  convergence  has  been 
ache iv  ed . 


p-l(-)  ■  £P-1(_)  ^  T  (3-43) 

Values  of  0  range  from  0  to  1.  When  0=0,  the  filter  does 
not  retain  memory  about  the  previous  states,  and  when  0  =  1, 
the  filter  retains  all  previous  data. 

The  Summary  for  the  Bayes  Filter  Algorithm  is  shown  in 


Appendix  I. 


IV  TESTING 


Computer  Program  Development 

The  computer  programs  were  developed  by  simply  coding 
the  equations  and  formulas  which  were  developed  in  the 
previous  sections.  The  programs  also  relied  on  basis 
programs  which  were  demonstrated  during  the  Modern  Methods  of 
Orbit  Determination  Course.  (Reference  7)  The  appendices  give 
a  brief  description  of  each  of  the  programs,  with  their 
inputs  and  outputs. 

To  ensure  the  programs  were  correct,  a  succession  of 
checks  were  run  to  determine  that  each  stage  of  the  program 
was  indeed  functional. 

The  first  check  was  of  the  numerical  integrator  .  A 
program  was  written  which  accomplished  this  by  numerically 
integrating  an  orbit,  once  around.  Appendix  A  details  the 
inputs  and  outputs,  and  the  procedures  used.  It  was  noted 
that  the  number  of  steps  was  crucial  in  solving  the  problem. 
A  step  size  that  was  too  large  could  not  be  used  with  a  high 
altitude  orbit,  or  an  elliptical  orbit.  The  starting  point 
for  the  integration  was  also  very  important  since  at  perigee, 
the  spacecraft  is  moving  faster,  thereby  requiring  smaller 
step  sizes.  Table  3  lists  numerical  integration  results  for 
an  orbit  giving  position  (DD)  and  velocity  (DD/TD)  vectors 
through  one  revolution.  If  the  numerical  integrator  were 
perfect,  and  there  were  no  roundoff  errors,  the  first  and 
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-.62125300577  -.08379947142  -.08379947142 

-.60150095501  -.13829660112  -.13819660112 

-.57226287487  -.19041428787  -.19041428786 

-.53399986734  -.23962902755  -.23962902754 

-.48731536291  -.28506467432  -.28506467431 

-.43294560409  -.32600468089  -.32600468087 

-.37174803446  -.36180339889  -.36180339887 

-.30468777626  -.39189626116  -.39189626114 

-.23282240982  - . 4 1 S80868528  -.41580868527 

-.15728529489  -.43316355805  -.43316355803 

-.07926769690  -.44368718275  -.44368718273 

.00000000002  -.44721359552  - . 44 72 1 359550 

.07926769694  -.44368718275  -.44368710273 

.15728529494  -.43316355805  -.43316355003 

.23282240987  -.41580868528  -.41500868526 

.30468777632  -.39189626115  -.39189626114 

.37174803452  -.36180339888  -.36180339886 

.43294560415  -.32600468087  -.32600468085 

.48731536297  -.28506467430  -.28506467428 

.53399906741  -.23962902752  -.23962902751 

.57226287494  -.19041428783  -.19041428782 

.60150095507  -.13819660107  -.13819660106 

.62125300583  -.08379947136  -.08379947135 

.63120752556  -.02008077392  -.02008077392 

.63120752555  .02808077410  .02808077409 

.62125300579  .08379947153  .08379947153 

.60150095500  .13819660124  .13819660123 

.57226287484  .19041420799  .19041428798 

.53399986728  .23962902768  .23962902766 

.48731536281  .28506467444  .20506467443 

.43294560397  .32600468100  .32600468099 

.37174803431  .36180339900  .36180339898 

.30460777609  .39189626126  .39189626124 

.23282240962  .41S80868537  41S80868S3S 

.15728529466  .43316355811  .433163SS009 

.07926769666  .44368718279  .44360718277 


xdot  ydot  xdot 

-.00000000028  .44721359554  447213S9S52 
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last  state  vectors  would  be  identical.  The  very  small,  10“^ 
errors,  represent  the  imperfections. 

Table  4  lists  sample  ouput  for  the  launch  trajectory. 
The  data  is  for  a  Titan  IIIB, launched  from  a  site  at  53.7°  N, 
158.2°  E.  The  initial  velocity  was  200  ft/sec  straight  up 
(local  coordinate  system),  and  to  displace  the  velocity  so 
that  the  vehicle  would  execute  a  gravity  turn,  this  initial 
value  was  perturbed  by  .06  DO/TD.  In  a  gravity  turn  there  is 
a  pre-programmed  displacement  to  perturb  the  vehicle's 
direction  so  that  gravity  will  cause  the  vehicle  to  fall  over 
and  reach  burnout  in  the  correct  orientation.  The  output 
contains  the  position  and  velocity  magnitudes,  y  ,  and  the 
time,  Ve  and  M.  Note  that  with  a  .06  displacement,  at  the  end 
of  data,  the  ICBM  is  virtually  horizontal  (y  =  80°),  and  in 
orbit.  This  is  easily  changed  by  altering  the  amount  of  the 
di spl acement . 

The  next  step  was  to  check  the  A  matrix.  Appendix  D 
lists  the  A  matrix,  and  the  program  which  accomplished  the 
check.  Given  an  initial  state  vector,  the  A  matrix  was 
calculated  .  Then,  each  element  of  the  input  state  was 
perturbed,  and  the  A  matrix  was  calculated  by  columns  as: 

F^x  +  8,t)  -  F  i  <  x  ,  t) 

A  =  -  (4-1) 

1J  6 

Table  5  lists  the  results  for  a  trial  case.  Note  that  each 
state  was  perturbed  by  about  1  x  10-^,  and  the  observed  delta 
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Tabla  4.  I  CBM  Lamaak  Trajaatory,  Nuarlatl  Imtagzatioa  Data 


imtia  '  stale  vector  for  the  missile  is 


-.4579093757416 e*00  -.562571: 

yaot  zdc 

-  .  43391  l2l64624e-02  .562653: 

the  initial  time  *s  .0020  Tl 
r  ( k m  >  v  ( ft/ sec ) 

6378.430588  263.814633 

6378.789127  326.359237 

6379.225596  393.765854 

6379.744871  466.471967 

6380.35:603  544.993952 

6381.050078  629.919834 

638  l  .844  101  721 . 896054 

6382.736897  821-610423 

6383.731040  929.773906 

6384.828431  1047.103783 

6386.030301  .174.310352 

6387.337255  1312.088445 

6388.749345  1461.114507 

6390.266165  1622.048931 

6391.886952  1795.543147 

6393.610703  1982.250598 

6395.436292  2182.340797 

6397.362579  2398.015809 

6399.388524  2628.528764 

6401.494541  2823.542549 

6403.655508  3027.078349 

6405.863128  3242.066483 

6408.129193  3468.884445 

6410.435582  3707.938547 

6412.784312  3959.672608 

6415.172571  4224.576523 

6417.597755  4503.195472 

6420.057503  4796.139981 

6422.549729  5104.097117 

6425.072654  5427.843164 

6427 . 624844  5768.258228 

6430.205238  6126.343318 

6432.313194  6503.240598 

6435.448525  6900.257717 

6438.111553  7318.897373 

6440.303161  7760.393661 

6443.524858  8220.257251 

6446 . 278862  3723.332209 

6449.063189  9248.868313 

6451.396772  9808.114288 

5454.769599  10404.939722 

6457.692997  11043.996945 

6460.674355  11730.939759 

6463.723417  12472.724757 

6466.851562  13278.035739 

6470.073319  14157.896975 

6473.405962  15126.585985 

6476.371492  16203.040581 

6480.497557  17413.120586 

6484.319703  18793.434422 

end  of  Jata  run 


-.562571 7569603e-00  .6883545 

zdot  Ve 

. 5626533647914e-02  .3172982 

.0020  TU 

(ft/sec)  gamma  (dag) 


2.8953242033 
4  .  4846479830 
6 .4  1  44524540 
8 . 6493455674 
1 1  . 1451502446 
13.8525358355 
16 . 7200663601 
19-6967954942 
22.7344241792 
25 . 7889803070 
28 . 8219788486 
31 . 8010569378 
34 . 7001278663 
37 . 4991401 939 
40.  18355  16549 
42 . 7436302088 
45  .  1736810688 
47 . 4712761083 
49.6365374388 
51 . 6896474704 
S3 . 6529187642 
55 . 5259659308 
57. 3093457140 
59.0043861052 
60-6130028733 
62 .  1375427256 
63.5806516542 
64 . 945166471  4 
66.2340269751 
67 . 4502059825 
68.5966544919 
69.6762594049 
70.691811 4876 
71  .6459815223 
72.5413028780 
73.3801589795 
74  .  1647743763 
74 . 8972082936 
75 . 5793496879 
76 . 2 1 291 291 90 
76.  7994331  9  1  5 
77 . 3402608925 
77  .  8365538476 
79  .  2892662990 
78 . 6991330153 
79.0666462800 
79 . 3920223679 
79  .  675  1  521  27  1 
79.9155267793 
80. 1  121229189 


756938e*00  - 

55 1 730e-00 

time  ( sec ) 
6136237488 
6136237488 
6136237488 
6 136237488 
6 1 36237488 
6 1 36237488 
6136237488 
6136237488 
6136237488 
6136237488 
6136237488 
6136237488 
6136237488 
6136237488 
6136237488 
6 1 36237498 
6 136237488 
6136237488 
6136237488 
6136237488 
6136237488 
6136237488 
61 36237488 
6 1 36237488 
6136237488 
6136237488 
6136237488 
6136237488 
6136237488 
6136237488 
6136237488 
6136237488 
6136237488 
6136237488 
6 1 36237488 
6136237488 
6 1 36237488 
6136237488 
6136237488 
6136237488 
6 l 36237488 
6136237488 
6 1 36237488 
6136237488 
6 136237488 
6 l 36237488 
6 1 36237488 
6 1 36237488 
6136237488 
6136237488 


»  dot 

353 103 7967 384 e-02 

M 

4479637558632e+01 

ve  (DU/~U> 
.3172982552 
.  3172982552 
.3172982552 
.  3172982552 
.  3172982S52 
.  31  72982552 
.3172982552 
.3172982552 
.3172982552 
.3172982552 
.3172982552 
.3172982552 
.3172982552 
.3172982552 
.  31  72982552 
.3172982S52 
.3172982552 
.  3172982552 
.  31  72982552 
.  3929044800 
.  3929044800 
.3929044800  3 

.3929044800  3 

.3929044800  3 

.3929044800  3 

.3929044800  3 

.3929044800  3 

. 3929044800  3 

.3929044800  3 

.3929044800  3 

.3929044800  3 

.3929044800  3 

.3929044800  3 

.3929044800  3 

.3929044800  3 

.3929044800  3 

.3929044800  3 

. 3929044800  3 

.3929044800  2 

.3929044800  3 

.3929044800  3 

.3929044800  3 

.3929044000  3 

.3929044800  3 

.3929044800  3 

.3929044800  3 

.3929044800  2 

.3929044800  3 

.3929044000  3 

.3929044000  3 


4 . 4796375586 
4 .4796375586 
4 .4796375586 
4  .  4796375586 
4.  4796375586 
4.4796375586 
4 . 4796375586 
4 .4796375586 
4  .  4796375586 
4.4796375586 
4  .  4796375586 
4  .  4796375586 
4  .  4796375586 
4 .4796375586 
4  .  4796375586 
4  .  4796375586 
4  .  4796375586 
4  .  4796375586 
4 .4796375586 
3 . 5272654570 
3.5272654570 
3 . 5272654570 
3  .  5272634570 
3 . 5272654570 
3.5272654570 
3 . 5272654570 
3 . 5272654570 
3.5272654570 
3 . 5272654570 
3.5272654570 
3.5272654570 
3-5Z72654570 
3.5272654570 
3.5272654570 
3.5272654570 
3. 5272654570 
3  .  5272654570 
3 . 5272654570 
3 . 5272654570 
3.5272654570 
3 . 5272654570 
3 . 5272654570 
3 . 5272654570 
3 . 52726545  70 
3  .  S2  726545  70 
3 .5272654570 
3 . 5272654570 
3 . 5272654570 
3. 5272654570 
3  .  5272654570 


13261 10*00*0*  -.6/697 0004**00  . 005 92 8000e*00  1 02 300000* -02 

.  4449U00g  (Jo -02  .6:2100001/6-02  .  3  1  7  2  9  8000e  +  0  0  4.479638  00  .  200000000®-0i 
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V  Results  and  Conclusions 


ICBM  Performance  Paraaeters  Estimation 

With  confidence  that  the  programs  are  functioning 
correctly,  as  obtained  from  the  results  in  the  last  chapter, 
the  original  problem  of  how  to  estimate  the  performance 
parameters  of  an  ICBM  in  flight  now  begins.  Due  to  time 
constraints  caused  by  heavy  computer  usage,  only  one  general 
case  was  examined,  however  the  trends  are  quite  certain  to 
extend  to  the  other  cases  which  were  programmed  into  the 
filter. 

Sample  test  results  are  listed  in  Appendix  J.  The  case 
which  is  presented  uses  100  data  point  segments.  Convergence 
is  shown  for  all  regions,  and  the  covariance  is  listed.  The 
case  uses  a  radar  site  at  52.6°  North,  174.1°  East,  1  DU  in 
elevation,  and  a  launch  point  at  4  3°  North,  132°  East.  A 
portion  of  the  truth  model  observation  data  for  the  test 
case  is  listed  in  Table  IS. 

Table  IS.  Truth  model  data,  ICBM  test  case 


range  azimuth  elevation  time 

(km) _ (deg) _ (deg) _ (sec) 


1065  .0411 

257 .08497 

-4  .7  879401 

2 .0136233 

1065  .0601 

257  .08497 

-4  .7  866484 

2.4131237 

1065 .0801 

257.08497 

-4  .7  8531  94 

2 . 8136233 

1065.0998 

257  .08495 

-4 .7  83  9529 

3  .2136237 

1065 .1196 

257 .08494 

-4  .7825492 

3  .6136232 

1065.1394 

257  .08494 

-4 .7  811076 

4.0136236 

,  ■  ■  — .  .  — 

The  truth  model  also  numerically  integrated  the 
trajectory  shown  in  Table  16.  Notice  that  only  the  first  200 


the  simulated  noisy  data,  with  the  covariance  data  for  both 
trials  listed  in  Table  13. 

The  tabulated  data  contains  the  corrections  to  the  state 
from  the  last  iteration  of  each  Bayes  loop.  Again,  all  units 
are  canonical  unless  specified.  It  is  interesting  to  note 
that  the  filter  converged  on  the  perfect  state  within  2 
iterations,  and  took  only  1  additional  iteration  to  converge 
on  the  other  cases  for  the  perfect  data.  Also  the  magnitude 
of  the  state  corrections  were  almost  all  10  which  is  very 
good.  As  with  the  least  squares  results,  the  data  converged 
almost  identically,  with  the  only  difference  being  the  amount 
of  time  required  to  reach  the  solution,  and  the  difference  of 
order  10_*  is  only  slightly  above  the  results  from  the  least 
squares  perfect  data.  The  covariance  matrices  are  again  very 
small,  with  the  error  ellipsoid  axis  lengths  decreasing  as 
the  filter  processed  more  data.  This  is  expected  since  the 
confidence  in  the  estimate  should  go  up  as  additional  data  is 
proce  s  sed. 

The  simulated  noisy  calculations  show  the  same  trend  as 
the  noisy  results  for  the  least  squares.  The  convergence  took 
about  one  extra  iteration,  and  the  final  errors  were  of  order 
10~3 ,  which  is  just  above  the  least  squares,  noisy  data 
results.  The  error  ellipsoid  axes  again  decreased  with  time, 
and  were  comparable  with  the  perfect  data  results. 


Table  14.  Bayes  Filter  Test  Results,  Noisy  Data 


iteration 
_ #  3 


CASE  #  1 


#  4 


#  4 


loop  1 


loop 


loop  1 _ 

. 1737332 e- 8 
. 5348930  e- 10 
.  7  90  527 1 e- 9 
. 244  8065  e- 8 
. 7611670 e-10 
. 1175545 e- 8 


. 3037524 e- 9 
. 1155548 e- 9 
. 4252 96 4 e-11 

-  .1910199e-8 

-  .4510833  e-9 
-.42930 26 e-10 


1 - 

-  .427  47  2  9  e-  9 

-  .1121525  e-9 
- .4957654e-10 

.  2261524  e-9 

-  .1429128e-10 

-  .3024271 e-10 


_  #  3 

Case  #  2 

_ #  3 

CASE  #  3 


it  4 


.  86  91772  e-9 

. 3  56  027  2  e- 9 
.3095512  e-9 
.71227  85  e-8 
. 2  991 415  e- 9 
. 1 6  9  841 5  e- 8 


.  63  3 1097  e- 9 
. 1 5  5  93  95  e-9 
.5717  885  e-9 
,  861 46 62  e- 8 
.8116607 e-9 
.4998575  e-9 


. 3037  522  e- 9 
.1155548 e-9 
.4252958 e-11 
-.191 0192 e-8 
-  .4510835  e-9 
- . 42  93  023  e- 10 


. 3  037  522  e- 9 
. 1155548e-9 
. 4252  93  9  e-11 
- .1910198e-8 
- .4510833  e-9 
-  .4293025e-10 


#  _ 4 _ 

-  .  4  27  47  2  8  e-  9 
-.11 21 525 e-9 

-  .4957650e-10 
.2261527  e-9 

-  .1429111e-10 

-  .3024270e-10 

#  4 _ 

- .4274728 e-9 

- .1121525  e-9 
-.4957651 e-10 
.2261527 e-9 
- .1429150e-10 

-  .3024260e-10 


#  4 


_ if  3 

CASE  #4 


8821206 e-7 
152487  4e-7 
27  99510  e-7 
88 827  3  9  e-7 
1766154 e-7 
3  0441 1 1 e-7 


a  4 


.3037518 e-9 
.1155548 e-9 
.4252  960  e-11 
. 1 910197  e- 8 
.  4  5 10  8  30  e-  9 
.42 93 025 e-10 


it  4 _ 

- .4274729e-9 
- .1121525e-9 
-.4957651 e-10 
.2261525 e-9 
- . 142  9134  e-10 
- .3024286  e-10 


the  final  corrected  states  are: 


_ T  ime  1 

.2499575  e  +  1 
-.2669495e-3 
-  .  81 22  9 94  e-4 


Time  2 
.2424369e+l 
.43  96  153  e  +  0 
.  43  8  84 43  e+0 


Time  3 
. 21 941 83  e  +  1 
.8536396 e+0 
. 8521426  e  +  0 


.2021882  e-2 
.44817  86  e+0 
.447066  8e  +  0 


1 57091 8e+0 
4336677  e+0 
4334313  e  +  0 


3 146  224  e+0 
3  864206  e  +  0 
3  892277  e  +  0 


i 


T4ble  13.  CoTirit&ca  data  for  Bayas  Filter  Testa  (continued) 
Noisy  Observation.  Data 


Covar lance  I 
.5130505a- 
- . 1095052e- 
1731172a- 
1731339a- 
. 108534 le- 
. 1243912a- 
. 0000000a* 
,0000000a* 
the  error  el 
.  1 37829e+01 
. 106405e-02 
. 000000a  +  00 
the  error  al 
. 1 56539e*0 1 
.  893599e-01 
. 536925e-01 


Matrix  at  epoch  Is: 


13  - . 1095052e-l3 
13  .2469536a-  1  4 

13  .  3835462a- 1 4 

13  . 2 1 35830a- l 4 

13  -  .  2429973a- l 4  -. 
13  - . 2439921e- l 4  -. 
00  . 0000000e+00 

00  . 000000 0e+ 00  . 

llpsold  axis  length 
maters 
meters 
maters 

llpsold  axis  length 
meters 
meters 
meters 


1781 172e-13 
38354G2e-14 
628 101 5e- 1 4 
4552 1 6 1 e- 1 4 
3752743e-  1 4 


1731339e-13 
2  1  8  5333e- 1 4 
4552161  a- 14 
33594350-13 
33872 1 3a-  1  4 


4224094e- 1 4  - . 36 1 6303e- ’ 4 
0000000e+00  .00000000+00 

0000000e + 00  . 0000000a +00 

is  for  the  covar lanca  matrix 


.  1035341a- 13 
.  2423973e-14 
.  3752743a- 1 4 
.  33372 1 3e- 1 4 
.  2500323a- 1 4 
.  2593239e- 1 4 
. 0000000e+00 
.00000006+00 
are 


lengths  for  the  position  components  are 


. 1 24391 2e- 
• .  243992 le- 
■ .  4  22  409 4e- 
■ .  3G  1  G303e- 
.  25932C9e- 
.3341942a- 
. 0000000e+ 
.000000061 


LOvar lanes  Matrix  at  epoch  Is: 

. 5689967e- 1 3  .2893223e-15  - . 7439639e- 1 4  -. 1 623987a- 1 3  .9627018e-14 

.  2393223a-  1 5  .U10523e-15  .  1536343e-16  -  .  1 1036  10e- 1  4  -  .  29  1  1  966e- 15 

-. 7439639e-14  .153G843e-16  .1080156e-14  .1241479e-14  - . 1 49G01 9e- 1 4 

-. 1623987e-13  1 103610a- 1 4  .1241479e-14  ,2469651e-13  .2350799e-14 

. 962701 8e- 1 4  - . 29 1 1 966e- 1 5  - . 1 49601 9e- 1 4  .2350799e-14  .3067733e-14 

.  1052807e- 1 3  - . 52 1 5 1 7 3e- 1 6  - . 1 55 4642e- 1 4  - . 1  1 8869  la- l 4  ,2271053a-14 

. 0000000e+00  . 00000006+00  . 0000000e+00  .00000006+00  ,0000000e+00 

.00000006+00  .0000000e+00  . 0000000e+00  . 0000000e+00  .00000006+00 

the  error  ellipsoid  axis  lengths  for  the  covariance  matrix  are 
.I47560e+01  meters 
•421826e-03  meters 
.421826e-03  meters 
.627327e-03  meters/sec 

the  error  ellipsoid  axis  lengths  for  the  position  components  are 
.I53441e+01  meters 
. 472052e-01  meters 
.8080006-01  meters 


. 1052307e- 1 3 
-.52151 73e-  1  6 
-. 1 554642e- i 4 
-. 1  1 3869 le-  1  4 
.2271 053e- 1 4 
. 2293 103e- 1 4 
. 0000000e+00 
. 0000000e+00 


I 


» 


Covariance  Matrix  at  epoch  Is: 

5294432a- 1 3  .1316609e-l3  .5108383e-14  -. 1 688632a- 1 3  ,82C9553e-14 

1 3 1 6609a- 1 3  .3348274e-14  .1315473e-14  -.47259.46-14  .lG94544e-14 

5103888a-14  . 13l5473e-14  .5723077e-l5  -. 201 9637a- 1 4  ,5399093e-15 

-  ! 633682“- 1 3  - . 47259 1 4e- L 4  - . 201 9637e- 1 4  .12132490-13  .147C759e-14 

. 3289553e- 1 4  ,1694544e-14  . 53990-98e- l 5  ,1473759a-14  ,3394437e-14 

. 8774566e- 1 4  .1954117e-t4  ,6101317e-15  -.15456026-15  ,3032544e-14 

0000000e>-00  .0000000e+00  ,0000000e  +  00  ,0000000e  +  00  .00000006  +  00 

,0000000e+00  . 0000000e+00  . 0000000e+00  .00000006+00  .00000006+00 

the  error  ellipsoid  axis  lengths  for  the  covariance  matrix  are 
.142217e+01  maters 
,S44872e-00  maters 
,3394G0e-03  meters 
.482731e-03  maters/sec 

the  error  ellipsoid  axis  lengths  for  the  position  components  are 
.1519006+01  meters 
. 63467 9e-01  meters 


.37745660-14 
. 19541 1 7e- 1 4 
. 6101317a- l 3 
-. 1 5  45602e- 1 5 
. 3032544e-  l  4 
. 2b52090e- 1 4 
. 0000000e+00 
. 0000000e+00 


» 


» 
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Tabl*  13.  CoTsriaaee  Data  for  Bayaa  Filter  Teats 
Perfect  Observation  Data 


Covariance  Matrix  at  epoch  is: 

. 1 24  5239e-l 3 
. 2440795a- 1 4 
. 4220799e-l4 
.36403679-14 
. 259231  la-14 
. 3O460G8e-l 4 
. 00000009*00 
.00000009+00 

the  error  ellipsoid  axis  lengths  for  the  covariance  matrix  are 
.137874e+01  meters 
•105428e-02  meters 
.0000009+00  meters 

the  error  ellipsoid  axis  lengths  for  the  position  components  are 
•156656e+01  meters 
.893346e-01  meters 
,586345e-01  meters 


185616e-13  1095270e-13  - . 1 78Z283e- 1 3  -.17384519-13  .1084102e-13 

-.10952709-13  . 2467963e- 1 4  .38347C7e-14  ,2199219e-l4  - . 2424386e- 1 4  - 

-.■.'322839-13  . 3334?37e- 1 4  .6202652e-14  .4572748e-14  -.37465509-14  - 

-.17204519-13  .219921 9e- 1 4  ,4572740e-14  .23679139-13  -. 2407667a- 1 4  - 

.10841029-13  -.2424CC6e-i4  -. 3746553c- 1 4  - . 2407 667e- 1 4  -2492565e-14 

.12452399-13  -.2440735e-14  - . 4226799e- 14  -. 3640367a- 1 4  .259231  1e-14 

. 9000000e+00  .00000009+00  . 0000000e+00  .00000009+00  . 0000000e+00 

.00000009+00  .00000009+00  . 0000000e+00  .00000009+00  . 0000000e+00 


Covariance  Matrix  at  epoch  is: 

.56367719-13  .2867172e-15  - . 74254 1 9e- 1 4  -.16187439-13  .9636801e-14  .1052316e-13 

.23671 72e- 1 5  .U06718e-15  .1562240e-16  - . 10989 1 6e- 1 4  - . 29046 72e- 1 5  -. 5265900a- 1 6  . 

-.74254190-14  .1562240e-16  .10766600-14  .1234741e-14  - . 1 494939e- 1 4  - . 1 552548e- 1 4  9 

- . 1613743e-13  - . 10939 1 6e- 1 4  ,1234741e-14  .2458098e-13  .2235350e-14  - . 1 1 303G4e- 1 4 

. 962680le- 1 4  -. 2904672c- 1 5  - . 1 494939e- 1 4  .2335358e-14  .20672109-14  .2274167e-14 

,1052816e-13  -.5265900e-I6  - . 1 552543e- 1 4  -. 1 1 80364c- 1 4  ,2274167e-14  .2294373e-14 

. 0000000e+00  . 0000000e+00  . 0000000e+00  .00000009+00  .00000009+00  . 00000000+00 

. 0000000e+00 ■  . 0000000e+00  . 0000000e+00  .0000000e+00  .00000009 +00  .00000009+00 

the  error  ellipsoid  axis  lengths  for  the  covariance  matrix  are 
.147528e+01  meters  • 

.421634e-03  meters 
,4216C4e-03  meters 
-625236e-03  meters/sec 

the  error  ellipsoid  axis  lengths  for  the  position  components  are 
.153395e+01  meters 
,471132e-01  meters 
,3J6310e-01  meters 

i 


Covariance  Matrix  at  epoch  is: 

.52624599-13  .1305212e-l3  .5072951e-14  - . I 654805e- 1 3  .3405727e-14  .380C699e-l4 

.13052129-13  .33114329-14  .1303232e-l4  -. 4632724a- 1 4  .17163509-14  .19O5901e-l4 

.50729510-14  .13032329-14  ,5682326e-15  -.198631)29-14  .5495307e-15  ,6121319e-15 

-,1654805e-13  -. 4633724c- l 4  - . 1 986832e- 1 4  .I200593e-13  .1510901e-14  - . 10659G5e- 1 5 

.34057279-14  .171 6350e- 1 4  ,5495807e-15  .15l0901e-U  .3956334e-14  ,3076475e-14 

.3G03699e-14  . 1955901e-14  .6121329e-15  - . 1 065 965e- 1 5  .3076475e-ll  ,2679237e-14  » 

,0000000e+00  .00000009*00  . 0000000e+00  .00000009+00  .00000000+00  .00000009+00 

.00000009+00  .00U0000e+00  . 0000000e+00  .00000009+00  . 0000000e+00  .0000000e+00 

the  error  ellipsoid  axis  lengths  for  the  covariance  matrix  3re 
.1417019+01  meters 

. 141731e+01  meters 
.547602e-00  meters 

.337014e-00  meters/sec  e 

.4727009-03  meters/sec 

the  error  ellipsoid  ax  1 3  lengths  for  the  position  components  are 
.1514169*01  meters 
.6343239-01  meters 
.3565449-01  meters 


I 
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Table  12.  Bayes Fil ter  Test  Results,  Perfect  Observations 


Corrections  to  the  state  from  the  last  iteration: 

I 

iteration  loos  1 

loon  2 

loop  3 

#  2 

#  2 

#  2 

-,3524090e-15 

.3775959e-15 

.7204185  e-16 

.1607043  e-1 5 

-  .3591601  e- 16 

.  1625420  e-16 

CASE  #  1  . 7 63026 1 e-1 6 

.4048324e-17 

. 84  56  7  22  e-1 7 

.7847741e-15 

-  .1044663  e-14 

- .6064243  e-15 

-  .  1693  56 2  e- 1 5 

.  6318910e-16 

- .4719534e-15 

-  .1939084e-15 

.3291155e-18 

-  .1421949e-15 

#  3 

#  2 

#  2 

-  .6998720e-13 

.  5343  816  e-1 5 

.7503202e-16 

.1195828e-14 

.143641 8e-15 

.7101780e-16 

CASE  #  2  . 1 97  4012  e- 14 

-  .6164276e-16 

.4390330e-16 

.  93  40  97  6  e-13 

-.1522799 e-14 

-.4117666 e-15 

.4582638e-14 

-  .29063  82 e-15 

-  .6517259e-15 

.4104247  e-14 

•6863711e-16 

-  .  1 882  97  7  e-1 5 

#  3 

#  2 

#  2 

.2342559 e-14 

.4178976e-15 

.26  50948e-15 

-  .  5523  967  e-15 

.6595235 e- 16 

.1126490 e-15 

CASE  #  3  - .1012912e-14 

-. 6635110 e- 16 

.33372  57  e-16 

-  .3881522e-14 

-  .1287997 e-14 

-  .8903416e-15 

.1229017  e-14 

-.1083272e-15 

-  .7648698e-15 

.2243644e-14 

.7944296e-16 

-  .1241624e-15 

#  3 

#  2 

#  2 

-.1011434  e-8 

.1709862  e-15 

.3549278e-16 

.57  867  53  e-9 

-.9786427  e-19 

-.5614404 e-17 

.76  91 572  e-9 

-.5444362  e-17 

.1247  841  e-16 

CASE  # 

\  .  1 7  885 90  e-  8 

-.3533691 e-15 

-  .1 854145  e-15 

-.8509662e-10 

.5039547e-16 

-.3  902  86  5  e- 1 5 

-.703  92 1  2  e- 9 

.407  8983  e-16 

-.6353  586  e-16 

The  final  corrected  states  are: 

Time  1 

Time  2 

Time  3 

. 2  500000  e  +  1 

.  2421457  e  +  1 

.  2190766  e  +  1 

.2511528 e-8 

.43  96257  e  +  0 

.  85 1 62  82  e+0 

-  .  1 9  26  930  e- 8 

.  43  96257  e+0 

. 85 162  82  e  +  0 

-.7 86 5713 e-8 

- .  1  57  2853  e  +  0 

- .3046  877  e+0 

.447  2135  e+0 

.  433  1635  e+0 

.  3  91 8  962  e  +  0 

.4472135e+0 

.4331635 e+0 

.3  91 896  2  e  +  0 

Table  11.  Least  Squares  Test  Results.  Noisy  Observations 


CASE  #  1  Corrections  for  iteration 

1 

.7924433  e-4 
.  107  7  90 3  e-4 
.  9042774 e-5 

-  .1686651e-4 

-  .  17  81 9  53  e-4 

-  .3911945e-5 

2 

-.4206869  e-8 

-  .3045  991 e-8 

-  .7908962e-9 
.1018113  e-8 
.  1 23  86  50  e-  8 
. 346 2006  e- 9 

3 

.1216706  e-11 

-  .1852942e-12 
.  24  821 18  e-11 

-  .4850761e-12 

-  .  9031411 e-13 
-.3183258e-12 

CASE  #  2  Corrections  for  iteration  # 


1 

2 

3 

4 

. 1 53  82  94  e-2 

. 40  96 174  e-4 

- .1647061e-7 

- .1540987e-10 

.26177  55e-5 

. 8171601 e-5 

-. 1336570 e-7 

.2158184e-ll 

- . 1334271 e-3 

. 1424602  e-3 

. 8914906  e-8 

- . 3  576  532  e-10 

-  .2302934e-5 

- .1456 163  e-4 

- . 93  6477  4  e- 9 

.6  372505  e-11 

.3163803  e-4 

- .4945985e-4 

. 3  5223  01 e- 8 

,5392571e-12 

-  .  3  960326  e-4 

. 3  56  953  2  e-4 

- .3660874e-8 

.4893  86  3  e-11 

CASE  #  3  Corrections  for  iteration  # 


1 

2 

3 

4 

. 6  927  874  e-4 

.9976133  e-5 

- .1474618e-7 

-  ,1859578e-ll 

.  1 84  84  99  e-4 

-.76  9977  3  e- 5 

- .9227  27  9e-8 

-.2557389e-ll 

-  .4008284e-4 

.4913705  e-4 

- .1221827e-7 

-.1033730e-ll 

-  ,1013049e-2 

- . 3  820  23  5  e- 5 

.3970187e-8 

.526 5535 e-12 

-  .323  8347  e-5 

- .1458009e-4 

,1520327e-9 

- .7616239e-13 

-  .  13  853  91 e-4 

. 993  82  50  e-5 

.4062703  e-8 

.6695699 e-12 

CASE  #  4  Corrections  for  iteration  # 


1 

2 

3 

4 

.1465780e-2 

.1135676 e-3 

-.1081471 e-6 

-.1593583  e-10 

-  .1008484e-l 

.9563121 e-4 

- .1354342e-7 

-.637  8967  e-11 

-  .1009848e-l 

.1076447 e-3 

-  .1127688e-6 

- .2897397e-10 

-  .9720443  e-3 

-.4485  862  e-4 

.3746891 e-7 

. 6  037  46  6  e-11 

.5100931e-3 

- .  806 1 36  8  e-4 

-  .2256346  e-8 

-.4146  842  e-12 

.4021184 e-3 

. 41 23  91 0  e-4 

.2633033  e-7 

. 5  92  53  3  0  e-11 

The  final  corrected  state  was: 


.250007  9e+l 
. 1077599 e-4 
.  90  41  9  86  e-  5 
-.1686550 e-4 
.4471957  e  +  0 
.44720968+0 


the  actual  solution.  Finally,  the  relative  magnitude  of  the 
differences  between  the  exact  and  the  estimated  state  are  all 
about  order  10-®  which  are  the  same  order  as  seen  in  the 
numerical  integration  checkout. 

To  complete  checkout  of  the  least  squares  filter,  the 
same  four  cases  were  tested  with  the  simulated  noisy  data 
from  the  truth  model.  Table  11  lists  the  results.  In  general, 
the  results  were  similar  to  those  with  the  perfect  data: 
however,  it  generally  took  longer  for  the  filter  to  eliminate 
the  noise  which  was  present  in  the  data,  and  the  final  state 
had  errors  of  order  10  This  represents  a  significant 
difference  to  the  least  squares  results.  As  can  be  seen  in 
Table  10,  the  covariance  matrix  was  also  the  same  for  all  the 
cases,  and  almost  identical  to  the  perfect  data  case. 

Bayes  Filter  Ckeekovt 

The  last  check  which  was  performed  was  to  check  the 
Bayes  filter  estimator.  Again,  for  consistency,  the  same 
truth  model  data  was  used.  Here  however,  there  was  an  effort 
to  limit  the  computational  time  due  to  extremely  heavy  usage 
of  the  computer,  therfore,  only  3  iterations  of  the  Bayes 
filter  were  tested,  with  each  of  the  runs  processing  30  data 
points.  This  wou'd  require  only  a  few  matrix  inversions,  and 
proved  adequate  to  show  trends  in  the  performance  of  the 
filter.  The  four  cases  were  tested  against  the  perfect  data, 
and  then  the  simulated  noisy  data.  Table  12  lists  the  perfect 
data  tests  and  Table  14  lists  the  Bayes  filter  estimations  of 
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Ttblr  10.  CoTtrltM*  Data  for  L«tit  Sqaaraa  Teat* 


PERFECT  OBSERVATION  OATA 


Covariance  Matrix  at  epoch  Is: 

. 1 894375a- 1 7  ,1906222e-L7  .1949674e-l7 

. 1 906222e- 1 7  .31056008-17  .1954685e-I7 

. 1 949674a- 1 7  . 1 954605a- 1  7  .3374542e-l7 

- . 7362794a- 1 8  -. 8456409a- 1 3  -. 9002667a- I  8 
- . 2880636a- 1 8  -. 339 484  7a- 1 3  - . 22497G7e-  1 8 
- . 2487960a- 1 8  - . 2068688a- 1 8  - . 33327 1 8e- 1 8 
. 0000000e+00  .000 0000a +00  . 0000000e+00 

,0000000a *00  ,0000000a +00  ,0000000a +00 


- . 7362794a-  1 3 
-. 8456409e-13 
- . 9002667a-  1  8 
. 3262 1 77a-  l  3 
. 1 080522a- 1 3 
. 9 78 4323e-  1 9 
,0000000a +00 
,0000000a+00 


- . 2830636e-l 8 
- . 3394  8  4  7a-  1 3 
-.2249767e-13 
. 1030523a-  10 
. 671971 5e-13 
. 1 457902a- 1 9 
,0000000a +00 
. 00000008+00 


the  error  ellipsoid  axis  lengths 
,772805e-02  meters 
. 565357e-05  meters 
,565357e-05  maters 
the  error  ellipsoid  axis  lengths 
.I65522e-01  meters 
. 38049Se-02  meters 
. 722636e-02  meters 


for  the  covariance  matrix  are 


for  the  position  components  are 


NOISY  OBSERVATION  DATA 


Covariance  Matrix  at  epoch  Is: 
,1894579e-17  ,1906361e-l7  ,1949884e-l7 

. 1 90636 le- 1 7  ,2l05715e-17  ,1954824e-17 

. I  949884a- l 7  .1554824e-17  .33748148-17 

- . 7362331e-13  - . 846S602e- l 8  - . 9003 1 64e- 1 8 
- . 2880884a- 1 8  - . 3395 i06e- l 9  - . 2250010e- 1 8 
-,2488251a- 18  -. 2068873a- 1 3  -.33330978-18 
.00000008*00  ,0000000e+00  .00000008+00 

.00000008+00  .00000008+00  .00000008+00 


-. 7363331 e- 18 
- . 8456602e- 1 3 
-.9003164e-13 
.32622458-18 
. 1 08059 1 e- 1 8 
. 9785 1 07e- 1 9 
.00000008+00 
. 0000000e+00 


-.2830884*  13 
. 3395 106e- 13 
.22500108-13 
. 1080591e-13 
,6719833e-19 
. 1453483e-19 
. 0000000e+00 
. 0000000e+00 


the  error  ellipsoid  axis  lengths  for  the  covariance  matrix  are 
.7728098-02  meters 
.5654098-05  meters 
.5654098-05  maters 

the  error  ellipsoid  axis  lengths  for  the  position  components  are 
,165529e-01  meters 
. 330508e-02  maters 
. 722700e-02  meters 


.2487960e-18 
.20636838-13 
.  333271 3e- 1 3 
.  9734323a- 1 9 
.  1  457902e- 1 9 
•5707239e-19 
.  0000000e*00 
.00000008+00 


.2483251e-18 
.  2063873a- 1 3 
.  3333097a- 1 8 
.97851 37e- 1 9 
. 145G403e-19 
. 57073G7e- l 9 
.0000000c +00 
•0000000e+00 


Tab 1 e9 . Le a  a t Sqna r e a  Teat  Eeaulta  Perf ectOba erwat iona 


CASE  #  1 

Corrections  for 

iteration  # 

1 

2 

.2121908e-8 

.64996380-15 

- .5513195o-9 

.I664450e-15 

- .7417027©-9 

.13571640-15 

-  .1253529e-9 

-  .98534500-16 

-  .2348516o-9 

-  .6637222  e-16 

CASE  if  2 

Corrections  for 

iteration  # 

1 

2 

3 

4 

.  1 46 120  5  e-2 

.3878021  0-4 

.16003  52  e-7 

. 13  50803  e-13 

- . 8012713  e-5 

.7986139e-5 

.2602269  e-7 

.2013430e-14 

- .1387883e-3 

.1387741 e-3 

.1345843  e-7 

.3  57  0665  e-13 

. 1370964e-4 

-.1370099e-4 

-  .  8527314  0-8 

- .53773100-14 

.4933  845  e-4 

-  .4934259e-4 

.4015420e-8 

- .7667879e-14 

- .  36  237  50  e-4 

.  3625131 e-4 

-.1405055o-7 

.287  9627  e-14 

CASE  #  3 

Corrections  for 

iteration  # 

1 

2 

3 

4 

- .1106544e-4 

,1106884e-4 

-  .12775820-8 

- .64574920-15 

•  7  545 843  e~5 

-  .7549170o-5 

.2775568e-8 

-.2021483  0-15 

- .50947  62  e-4 

,5095004e-4 

-. 315484  5 e- 8 

-.443  8048e-15 

-  .9957  467e-3 

-  .42533970-5 

.22978460-9 

.28716130-15 

.  1 46  2024  e-4 

-.1462056e-4 

.1873276e-9 

- .5522853  e-16 

-  .9643821e-5 

.  9643  8340-5 

-  .2476946e-9 

.2050013e-15 

CASE  H  4 

Corrections  for 

iteration  # 

1 

2 

3 

4 

.  1 3  8852  8  e-2 

.1115167 e-3 

-  .4337689e-7 

.987 5105 e-14 

-  .  1009635  e-1 

.  9632617e-4 

.31661930-7 

.52269980-14 

-  . 1 0103  82  e-1 

.1038821e-3 

-  .  541 991 2  e-7 

.2427  022  e-13 

-  .9561011e-3 

-  .4391646e-4 

.  17762  85  0-7 

- .3653104o-14 

.52786040-3 

-  ,8056l49e-4 

-. 3599163 e- 8 

- .9022306e-14 

.40552740-3 

.4176069e-4 

.7124101 e-8 

. 5774726e-14 

The  final 

corrected  state 

was  equal  to : 

. 2  500000  e  +  1 
- .5512194e-9 
- .7417025  e-9 
.  1 2 83  6  54  e-  9 
.4472135  e  +  0 
. 4472135 e+0 


Each  of  the  cases  was  ran  with  the  least  squares  filter 
and  the  results  are  shown  in  Table  9.  (units  are  canonical 
and  only  6  states  are  shown  since  the  satellite  data 
consisted  only  of  position  and  velocity  vectors.)  Note  that 
the  filter  converged  in  only  2  iterations  on  the  perfect  data 
case,  yet  it  required  4  iterations  on  the  perturbed  initial 
state  vectors.  In  each  case,  the  final  state  was  the  same 
since  the  estimator  was  using  the  same  truth  model  data.  The 
filter,  after  processing  all  500  data  points  of  one  period  of 
the  orbit,  improves  the  initial  guess  by  a  very  small  amount, 
10~^,  which  can  really  be  treated  as  truncation  and  machine 
error.  A  measure  of  the  accuracy  of  the  filter  is  the 
covariance  matrix,  from  which  the  error  ellipsoid  axis 
lengths  can  be  determined.  The  procedure  here  is  to  calculate 
the  eigenvalues  of  the  covariance  matrix.  The  square  root  of 
each  eigenvalue  is  then  the  error  ellipsoid  axis  length. 
Table  10  lists  the  covariance  matrix  and  the  error  ellipsoid 
axis  lengths.  The  covariance  was  the  same  for  all  the 
different  cases  using  the  same  truth  model  data,  which  simply 
means  that  the  same  degree  of  confidence  can  be  placed  with 
each  estimate.  This  of  course  assumes  that  the  dynamics  model 
is  valid.  The  second  set  of  axis  lengths  are  considered  to  be 
more  accurate  since  they  represent  the  upper  left  3x3 
partition  of  the  covariance  matrix.  This  is  the  best  measure 
of  the  accuracy  of  the  position  components.  The  filter  seemed 
to  adapt  rapidly  to  the  errors,  correct  them,  and  converge  on 


The  truth  model  program  generated  the  numerical 
integration  data  shown  in  Table  3  and  also  produced  the  truth 
model  (range,  azimuth  and  elvation)  data  for  the  orbit. 
Table  7  lists  the  first  few  lines,  and  the  last  few  lines  of 
the  truth  model  data  as  it  was  used,  (both  perfect  and  noisy 
data  simulations  are  included) 


Table  7.  Truth  model  data  for  satellite  orbit 


simulated  perfect  observation  data 

""  range  az  imuth  elevation 

(DU)  (rad)  (rad) 

2.10828311  5.43568181  .190435864c 

2.09648962  5.43864117  .203551598e 

2.08473224  5.44165050  .216743101e 

2.07301339  5.44471208  .230011201e 


(rad) 

.19043  5864e  +  0 
.203551598  e+C 
.2167  43 101 e+0 
.  230011201 e+0 


time 

(TU) 

.496729413  e-1 
.993458827 e-1 
.149018824e+0 
,19  86  91765  e+0 


2  .29091161 
2 .27868763 


5.21078168  -  .  71457  8847  e-3 
5.21196854  . 11 50 804 26 e-1 


. 2  46  87  451 8  e+2 
.2  47  371248  e  +  2 


simulated  noisj 
2.10828823 
2  .09648944 
2.08473754 
2  .0730343  9 


observation  data: _ 

5.43455972  I  . 1 8974 83 49 e+0 
5.43893145  .204563277 e+0 
5.44310397  . 2 162 1 5 5 98 e+0 
5.44533297  . 23 11 986 7 8e+0 


.496729413  e-1 
.  9  93  45  8  82  7  e- 1 
. 14  9018  824  e  +  0 
.198691765  e  +  0 


2.29090397  5.21055858  . 94021 9542 e-3  .246 87451 8e+2 

2.27869430  5.21154640  . 127423 844 e-1  .  2 47 37 12 4  8 e  +  2 

For  each  test,  four  cases  of  initial  input  data  vectors 


were  run.  Table  8  summarizes  the  test  cases. 


Table  8.  Filter  test  cases 


case  1 

case  2 

case  3 

case  4 

. 2  500000  e+1 
.0000000  e+0 
.0000000  e+0 
.0000000  e+0 
.447  213  5  e+0 
. 447 2 13  5  e+0 

. 2498000  e+1 
. 0000000  e  +  0 
.0000000e+0 
.0000000  e+0 
.4472135  e+0 
.447  213  5  e+0 

. 2  500000  e  +  1 
.0000000  e  +  0 
. 0000000  e  +  0 
. 1000000  e-2 
.447  213  5  e+0 
. 4  47  2 1 3  5  e+0 

.249°  00  e+1 
. 1000000  e-2 
. 1 000000  e-2 
. 1000000  e-2 
.446  7  6  63  e+0 
.4467663  e  +  0 
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It  is  important  to  realize  that  all  the  calculations  must  be 
made  in  the  IJK  frame.  There  are  several  places  in  the 
programs  where  conversions  between  SEZ  and  IJK  frames  are 
performed,  and  later  when  runs  are  made  with  the  Least 
Squares  and  the  Bayes  programs,  their  input  data,  in  order  to 
be  consistent,  must  be  all  in  the  IJK  frame. 


Least  Squares  Checkout 

The  next  step  was  to  run  a  trial  program  of  Least 
Squares.  This  would  serve  only  to  check  the  combination  of 
all  the  different  matrices  and  formulations  that  were 
developed  thus  far.  The  program  estimates  the  original  state 
vector,  given  only  an  initial  guess,  and  the  range,  azimuth, 
elvation,  and  time  data  from  the  truth  model.  The  program  is 
listed  in  Appendix  H.  For  consistency,  all  of  the  test  cases 
for  the  Least  Squares  and  the  Bayes  filter  were  identical, 
using  the  following  input  data. 


Table  6.  Test  Cases,  Input  Data 


Satellite  orbi t 


a  =  2.5  DU 
e  =  0 

i  =  45  deg 
ft  =  0  deg 

u>  =  0  de  g 

M  = 


radar  site 
45  deg  North 
60  deg  Vest 
1.0225DU  elevation 


dt  =  period/500  TU 
tQ  =  0  TU 

initial  velocity  200  ft/sec 
displacement  of  .06  DU/TU 


in  the  calculated  matrix  was  also  about  1  x  1  O  '* . 

Next,  the  ^  matrix  was  calculated  and  checked  .  See 
Appendix  E  for  the  checkout  program.  This  was  accomplished  by 
first  setting  the  state  vector  to  an  initial  value  and  saving 
it.  The  state  was  then  moved  through  time  (which  worked  out 
to  be  about  of  the  orbit),  and  again  saved.  Individually 
then,  each  of  the  elements  of  the  state  vector  were  perturbed 
at  the  original  time  and  translated  through  time.  The  columns 
of  the  ^  matrix  could  then  be  calculated  as: 


X  (  x  ( 1 0 )  +8  ,  t)  -  X  (  x  ( 1 0  )  ,  t) 


(4-2) 


It  should  be  noted  here  that  after  each  perturbation,  the 
state  and  the  ^  matrix  were  reinitialized.  Table  5  lists  the 
output  and  shows  that,  for  input  errors  to  the  state  vector 
of  about  1  x  10-*,  the  same  order  of  magnitude  errors  were 
observed  in  the  calculated  matrix. 

Once  the  G  and  H  matrices  were  programmed,  a  check  was 
performed  on  the  H  matrix.  The  program  is  listed  in  Appendix 
F.  The  state  vector  is  input,  and  one  call  to  obser 
calculates  the  H  matrix  directly.  Then,  similar  to  the  check 
for  the  A  matrix,  each  one  of  the  state  vector  elements  are 
perturbed,  and  the  columns  of  the  H  matrix  are  calculated  as: 


G  ( x  +8,  t  )  -G(x  ,  t) 


t  H  ] 


(4-3) 


Tablo  If.  Nmaari osl  Zatagxatlo*  of  ICBH  Toot  Coso 


Tho  Initta  state  sector  for  tho  imsallo  fs 

<  y  X  xdot 

-.13261130912940-00  -.57696953521210-00  . 805928282Z486o-00  - . 1022S944089S3O-02 

ydot  zdot  Vo  M 

-.44491365383250-02  .62760336913600-02  .31729825517300-00  .44796375586320-01 

tho  initial  tfmo  <a  .0020  TO 

r  (km)  w  (f t/soc)  gamma  (dog)  1 1  mo  ( soc )  vo  (DU/'rU> 

6378.425311  259.243619  .4765268340  5.6136237488  .3172982552 

6378.779878  321.528788  .7435895547  9.6136237488  .3172982552 

6379.211102  388.369675  1.0696148558  13.6136237488  .3172982552 

6379.727641  459. 996128  1.4495086812  17.6136237488  .3172982552 

6380.334418  536.656672  1.8770713579  21.6136237488  .3172982S52 

6381.037640  618.620109  2.3435806240  25.6136237488  .3172902552 

6381.843819  706.177191  2.8*81994209  29.6136237488  .3172982552 

6382.759789  799.642473  3.3782525931  33 . 6 1 36237488  .317 2982552 

6383.792733  899.356393  3.9294071189  37.6136237480  ‘.317Z982SS2 

6384.950208  1005.607698  4.4957822772  4 1 . 6 1 36237488  .31 72902552 

6386.240181  1119.036281  5.0720095664  45.6136237488  .3172982552 

6307.671061  1239.836538  5.8532571069  49.6136237408  .3172902552 

6389.251743  1368.561347  6.2352294052  53.6136237488  .3172982552 

6390.991655  1505.726783  6.SM1S04768  57.6136237488  .3172982552 

6392.900812  1651.897727  7.3867361718  61.6136237488  .3172982552 

6394.989882  1807.694513  7.9501599517  65.6136237488  .3172982552 

6397.270250  1973.800849  8.502015175B  69.6136237488  .3172982552 

6399.754111  2150.973260  9.0402760696  73.6136237488  .3172982552 

6402.454552  2340.052397  9.5632588935  77.6136237488  .317298255 2 

6405.356695  2490.305197  10.0747264442  81.6136237488  .3929044600 

6408.436116  2645.832961  10.5795952776  85.6136237488  .3929044000 

6411.701670  2809.651417  11.0765767476  89.6136237488  .3929044800 

6415.162763  2982.238499  11.5645042960  93.6136237488  .3929044800 

6418.829345  3164.112753  12.0423381122  97.6136237488  .3929044800 

6422.711963  3355.839115  12.5091584371  101.6136237488  .3929044800 

6426.821817  3558.034961  12.9641583753  105.6136237488  .3929044800 

6431.170828  3771.377151  13.4066362980  109.6136237488  .3929044800 

6435.771714  3996.610301  13.8359880170  113.6136237488  .3929044800 

6440.638078  4234.556504  14.2516988823  117.6136237488  .3929044800 

6445.784503  4486.126839  14.6533359155  121.6136237488  .3929044800 

6451.226663  4752.335075  15.0405400725  125.6136237488  .3929044800 

6456.981461  5034.314065  15.4 13*186941  129.6136237488  .3929044800 

6463.067170  5333.335523  15.7705381906  133.6136237488  .3929044800 

6469.503616  5650.834031  16.1129169784  137.6136237488  .3929044800 

6476.312379  5988.436427  16.4400186731  141.6136237488  .3929044800 

6483.51 7043  6347.998092  16.7517455262  145.6136237488  .3929044800 

6491.143400  6731.648165  17.0480320723  149.6136237488  .3929044800 

6499.220206  7141.846485  17.3288389399  153.6136237480  .3929044800 

6507.778798  7501.456101  17.5941467558  157.6136237488  .3929044000 

6516.854418  8053.836767  17.8439500500  161.6136237488  .3929044800 

6526.486452  8562.967167  18.0782510354  1 65 . 6 1 3623 7488  .3929044800 

6536.719313  9113.607185  18.2970530935  169.6136237488  .3929044800 

6547.603469  971  1.517058  1 8 . 5003537347  1 73 . 6 1 36237488  .3929044800 

6559.196754  10363.759207  18.6881367086  177.6136237408  .3929044800 

6571.566111  11079.123248  18.8603627994  181.6136237488  .3929044800 

6584.789908  11868.739931  19.0169586114  185.6136237488  .3929044800 

6598.961134  12746.994767  19.1578022810  189.6136237488  .3929044800 

6614.191898  13732.936069  19.2827044076  193.6136237488  .3929044800 

6630.620006  14852.537972  19.3913813462  197.6136237488  .3929044000 

6648.418941  16142.528699  19.4834158043  201.6136237488  .3929044800 


m 

4.4796375586 

4.4796375586 

4.4796375586 

4.4796375586 

4.4796375586 

4.4796375586 

4.4796375586 

4.4796375586 

4.4796375586 

4.4796375586 

4.4796375586 

4.4796375586 

4.4796375586 

4.4796375586 

4.4796375586 

4.4796375586 

4.4796375586 

4.4796375586 

4.4796375586 

3.5272654570 

3.5272654570 

3.5272654570 

3.5272654570 
3.5272654570 
3.5272654570 
3.5272654570 
3.5272654570 
3 . 5272654570 
3.5272654570 
3.5272654570 
3 . 5272654570 
3.5272654570 
3.52726545 70 
3.5272654570 
3.5272654570 
3.5272654570 
3 . 5272654570 
3.5272654570 
3.5272654570 
3.5272654570 
3.5272654570 
3.5272654570 
3.5272654570 
3.5272654570 
3.5272654570 
3.5272654570 
3.5272654570 
3.5272654570 
3.5272654570 
3.5272654570 


r 
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f* 

l~ 
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seconds  are  shown  since  it  was  desired  to  limit  the 
computational  time.  The  staging  event  between  stage  I  and 
stage  II  could  then  be  observed. 

A  main  concern  was  to  show  that  the  filter  could  not 
only  observe  the  first  stage  parameters,  since  it  would 
surely  work  on  these,  given  the  results  of  the  last  chapter, 
but  that  it  could  also  observe  the  staging  event,  and 
continue  to  estimate  the  second  stage  parameters  as 
additional  data  was  processed.  If  the  filter  could  observe 
the  staging  event  of  the  first  and  second  stages,  it  would 
also  be  able  to  observe  the  staging  events  throughout  the 
rest  of  the  flight. 

Several  test  cases,  such  as  those  listed  in  Appendix  J 
were  run.  It  was  desired  to  have  the  filter  retain  good 
memory  about  the  position  and  velocity,  since  the  changes  in 
these  quantities,  even  during  staging  events,  would  be  small. 
However,  the  and  M  would  be  changing  rapidly  only  at  the 
staging  events.  To  account  for  this  phenomena,  the  test  cases 
were  run  with  fi  values  of  .8  -  1.0  for  the  position  and 
velocity  vectors,  and  .5  -  .7  for  the  Ve  and  M.  The  effect  of 
this  was  to  have  the  filter  try  to  throttle  the  ICBM  in 
order  to  make  the  estimation  correct  when  it  was  close  to  a 
staging  event.  The  compiled  data  is  graphed  in  Figures  6  thru 
14,  and  shown  numerically  in  Appendix  J.  Notice  that  in  the 
figures,  the  best  results  are  shown  in  Figures  6  and  10 
(exhaust  velocity  and  mass  ratio  respectively),  and  the 
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j  100  data  points 

2  66  data  points  ** 

3  Stage  I  actual 

4  Stage  II  actual 


Data  Points 


Exhaust  Velocity  (DU/TU) 


Data  Points 

Figure  8.  Test  Cases:  24,  66 
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16  data  points 


50  data  points 

Stage  I  actual 
Stage  II  actual 


_J _ 

100 


Data  Points 


Pigure  13.  Test  Cases 


poorest  performance  are  shown  in  Figures  9  and  14. 

It  was  determined  that  the  filter  was  only  able  to 
observe  the  staging  event  successfully  when  the  event 
occurred  within  one  of  the  Bayes  loop  segments  of  data.  In 
add ition,  the  staging  event  needed  to  occur  in  the  latter 
portion  of  the  data,  and  the  more  stage  I  data  that  was 
present,  the  better  the  performance  of  the  filter.  Table  17 
lists  the  summary  of  the  data  segments  which  were  run. 


Table  17.  Summary  of  Data  Points 


#  Of 
points 

Staging  event 
data  segment 

■n 

#  of  Stg  II 
points 

Ratio 

St  g  1/  St  g  II 

16 

176-192 

14 

2 

7 

24 

168-192 

22 

2 

11 

32 

160-192 

30 

2 

15 

48 

144-192 

46 

2 

23 

50 

150-200 

40 

10 

4 

64 

128-192 

62 

2 

31 

66 

132-198 

58 

8 

7.25 

100 

100-200 

90 

10 

9 

*  Note  that  the  staging  event  occurs  at  point  #  190 


Notice  that  the  smallest  ratio  that  successfully  converged 
was  the  50  point  case.  Ratios  of  less  than  4  did  not 
converge.  This  was  apparently  due  to  the  fact  that  the 
estimator  could  not  change  the  performance  parameters 
instantaneously,  as  the  stage  effectivly  does.  Therefore,  it 
seemed  to  need  a  good  memory  (the  previous  stage's  data), 
along  with  a  little  new  data,  to  enable  a  partial  correction 
of  the  data.  On  the  next  data  segment  which  was  processed, 
the  entire  set  of  stage  II  data  enabled  the  further 


» 
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refinement  of  the  estimate,  and  in  most  cases,  was  very  close 


to  the  actual  value. 

A  further  correlation  existed  between  the  ratio  and  the 
performance  of  the  estimator.  Careful  examination  of  the 
figures,  and  the  information  in  Table  17  shows  that  the 
higher  ratio  numbers  produced  the  most  accurate  results.  The 
case  which  used  64  data  points  for  each  segment  very  closely 
approximated  the  actual  data  for  both  exhaust  velocity,  and 
mass  ratio,  and  it  had  the  highest  Stg  I/Stg  II  ratio.  This 
further  supports  the  contention  that  the  filter  needed  to 
process  the  data  slightly  differently  during  the  staging 
event  in  order  to  assure  success. 

Examination  of  the  figures,  and  the  data  for  the  test 
cases  run  with  66  data  points  indicate  a  trend  with  the  p 
values.  The  case  which  included  p  values  of  .9  and  .7  seemed 
to  perform  a  little  better  than  the  case  with  p  equal  .8  and 
•S  .  This  tends  to  indicate  that  the  filter  needs  to  keep  a 
reasonable  amount  of  memory  to  correctly  estimate  the 
performance  parameters.  The  value  at  .7  does  this,  allowing 
for  just  enough  change  to  incorporate  the  variations  caused 
by  the  staging  event. 

The  figures  and  the  data  show  that  the  filter  estimated 
the  performance  parameters  very  well  during  most  of  the 
stages  thrusting.  As  the  staging  event  was  approached 
however,  each  of  the  cases  diverged,  proceeded  to  initiate  an 
estimate  of  the  second  stage's  performance,  and  then 
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converged  on  the  second  stage's  performance. 

Conclusions 

A  method  for  the  estimation  of  launch  vehicle 
performance  parameters  and  the  position  and  velocity  vectors 
has  been  examined.  The  specific  parameters  were  exhaust 
velocity  and  mass  ratio.  The  data  obtained  from  the  test 
cases  shows  that  the  filter  will  estimate  the  performance 
parameters.  The  occurrence  of  the  staging  event  within  a 
segment  of  data  did  not  appear  to  present  a  problem  as  long 
as  the  event  occurred  significantly  towards  the  end  of  a 
particular  data  segment.  This  provided  a  means  by  which  the 
filter  could  perform  a  staged  change  from  one  state  to 
another . 

Recommendations  for  additional  work  could  be 
accomplished  by  using  data  already  generated  at  this  point. 
The  added  features  could  include  a  form  of  residual 
monitoring  in  the  program  which  would  watch  for  a  quadratic 
departure  in  the  in-track  position  residuals.  This  would 
suggest  the  probability  that  the  time  was  close  to  a  staging 
event.  The  program  could  then  shift  the  data  segment  so  that 
the  staging  event  would  always  occur  towards  the  latter 
portion  of  the  data.  This  also  suggests  the  possibility  for 
iterating  back  over  the  staging  event  to  try  and  refine  the 
guess.  Another  change  could  include  an  alteration  of  the 
accuracy  of  the  measurements.  These  could  be  altered  to 
determine  if  there  is  a  limit  for  the  resolution  of  the  radar 


and  infared  system  that  must  be  obtained  to  allow  for 
convergence.  Finally,  additional  values  could  be  input  for 
the  0  values  to  see  if  additional  changes  would  alter  the 
results  obtained,  especially  with  the  smaller  data  segment 
size  (10-30).  The  overall  objective,  however,  of  observing 
exhaust  velocity  and  mass  ratio  was  successful. 


pr  frit*.  *  y*  ’  .  y  I  1  ,nxt  ’  *6378 .  1  45d+00  ,y(2,nxt)*6373.  I  45d 

*  *00, y< 3 . nxt  >  *6378 . 1 45d*00 , y { 4 . nxt ) *7 . 90536828d+00 
print*. ’y-’ . v ( 5 , nxt  ■  *7 . 90536828d+00. y ( G . nxt > *7 . 90536828 

*  d*00, y < 7 , nxt ) . y I  8 , nxt > 

write! 17,2) 

ccccc  begin  loop  to  Integrate 

do  10  Incc*  O.nlt 

ca 1 1  hami ng { nxt  > 

Inb*  f nb  ♦  1 
do  30  1  nd* 1 , 3 

r I  I nd  )  “  yllnd.nxt) 
vllnd)*  y(1nd*3.nxt) 

0  continue 

call  razel (r,v, r ho , az , el , to , t , r s , trm , Ino) 

If  ( loh . ne . 10)  then 

print*, ’do  you  want  noisy  data.y  or  n* 
read" , type 

If  I  type ,eq . ’ y * >  then 

pr I nt* , " I nput  a  seed  number  from  1-21 483647d+00* 
read* .dseed 
end  I  f 
loh*  10 
end  If 

If  (type.eq. ’y’ >  then 

rho*  rho  ♦  ggnqf I dseed ) *s Igr ho 
az*  az  *  ggnqf (dseed ) *s Igaz 
el*  el  +  ggnqf (dseed )*s Igel 
end  I  f 

wrlte(14.*l  rho.az.el.t 

If  (  Inb.eq. 10)  then 
do  34  I ns*l  . 3 

rm(lns)  •  r  I  I  ns ) "6378 . 1 4Sd+00 
vm( Ins)  *  v( 1ns)*25936.2647d*00 
34  continue 

call  mag(r) 
call  mag(rm) 
call  mag(vm) 
cal  1  mag( v  ) 

dot  -  r ( 1 >*v( 1 )*r(2)*v(2)*r(3)*v(3) 
gamma  *  dacos < dot/ 1 r 1 0 ) * v 1 0 ) ) ) 
tm-  t*806 . 8U8744d+00 

wr I  tel  1 7 , 6 )  rm(0) , vml 0) , gamma /rad ,tm,y(7,nxt) ,y(8,nxt) 

I  nb*  0 
end  I  f 

If  (  I ncc . eq . n 1 1- l  1  then 

zccccc  print  out  f t na 1  data 

print*, ’the  final  values  for  r  and  v  are*’ 

pr tnt*,  'y- ’ ,y( 1 , nxt) *6378. 1 45d  +  00 , y ( 2 . nxt > *6378 . 14Sd*00 

*  ,y(3, nxt) *6373. 1 45d*00 . y ( 4 , nxt ) *7 . 90536828d*00 
print*. ’y- ’ . y(5,nxt>*7 . 905 36828d+00 . y I  6 , nxt > *7 . 90536828 

*  d*00 , y I  7 , nxt ) , y < 8 , nxt ) 
wr I  tel  1 7 , 1 2  1 

wr I  tel  17,4)  (y(tnf,nxt),lnf*l,8),t 
write! 1 7 , *  > 
end  I  f 

10  continue 

end  fl 1  el un 1 1» 17 ) 
endfl le(unlt*14) 


pr 1 nt* ,’ i nput  the  launch  point  number’ 
read*  ,  1  p 

If  (  Ip  .eq  .0)  then 

1 1 at»53 . 7d*00*rad 
1  1on-158.2d-00*rad 
end  1  f 

If  ( 1 p . e c • 1 1  then 

1 1 at»43 . 5d*00*r ad 
1 Ion-132. 0d-00*rad 
end  1  f 

.If  (  1  p  . eq  •  2  )  then 

1 lat-1 .0d+00*rad 
1 lon-1 .0d+00*rad 
end  If 

If  < 1 p . eq . 3 )  then 

1 1 at» 1 .0d+00*rad 
1  lon-l .0d*00*rad 
end  If 

1 r s ( 0  > “ 1 .0d*00 

call  1 st 1  me! 1 1 st , t , to , 1  Ion ) 

ans-  ’  1  ’ 

call  radst< lrs, 1 lat, 1 lst,t.to,ans, Ino) 

1  no-0 

pr 1 nt* , *  1 nput  Initial  velocity  In  ft/s’ 
read* , 1 vel 

I vel «  lvel/25936. 247G4d*00 
do  40  1 nr-1 , 3 

40  1  vv < 1  nr ) -  1 vel *  1 r s ( I  nr  ) 

pr!nt*,’how  much  do  you  want  to  nudge  the  velocity’ 
read* , nudge 

lvv<3>-  1  vv ( 3 >  + 1 vv 1 3 ) "nudge 
do  44  1 nv*  1 , 3 

y( 1nw,nxtl»  lrs(lnw) 
y ( 1 nw*3 , nxt ) ■  lvv(lnw) 

44  continue 

end  If 

tp-  tp/806 .811 8744d-00 

pr 1 nt* , ’ 1 nput  the  number  of  Iterations’ 
read*. n 1 t 
dt«tp/n 1 t 
nxt-0 

cccccc  write  Initial  header  data 

wrtte(17,*)  "The  initial  state  vector  for  the  missile  Is’ 
wr 1  tel  17 . 12) 

l  nb  -  0 

dseed-  38888. d+00 
slgrho-  . 00001 d ♦00 
slgmz-  .001d»00 
slgel-  .00 ld*00 
type-  ’n’ 

cccccc  Initialize  hamlng  and  reset  the  time 
nxt-0 

ca 1 1  ham  I ng ( nxt ) 
t»  tepoch 
If  (nxt.eq.0>  stop 

wr I  tel  1 7 , 4 )  (y(  Inf, nxt). Inf-1, 3), t 

pr 1 nt* . y 1 1 .nxt) ,y<  2 .nxt) . y 1 3 . nxt ) ,y<  4. nxt) ,y<5.nxt> .ylS.nxt 
♦  y ( 7 , nxt ) . y< 8 , nxt ) 


cccccccccccccccccccccccccccccccccccccccccccccceecccccccccccccccceccccccc 

c  Thfs  program  obtains  the  truth  model  data  for  the 

c  problem  of  the  launch  vehicle.  The  program  Is  set  up 

c  for  several  different  types  of  vehicles. 

c 

c  Capt.  Dave  Vallado  1984 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 
cccccc  the  common  terms 

common  /ham/  t , y ( 72 . 4 ) , f ( 72 . 4 ) , er r ( 72 ) , n , dt . mode 
double  precision  t,y,f,err,dt 
Integer  n, nxt, mode 

cccccc  all  the  other  variables 

1 nteger  nit. 1 na , 1 nb . Incc , Ind , 1 ne, Inf, 1  no 

double  precision  s Igaz .tepoch , Ip , 1 1  at , 1  Ion , 1 rs (0:3 ). 1 vel , 

♦  1 1 st , rad . tp , rho, az , el , to , d seed .slgrho, 1 vv ( 0 : 3 ) , nudge, 

♦  r (0:3) . v(0:3> ,rs<0:3> .rev (0:3) ,trm(3 ,3) , slgel , 

♦  rm<0:3) , vm(0:3> , dot, gamma, tm 
character  type, ans , typed 

cccccc  begin  the  program 

2  format(Sx,’r  (km)',8x,'v  ( ft/sec )’, Sx ,’ gamma  (deg)’,6x, 

+  ’time  ( sec ) ’ , 4x , ’ ve  ( DU/TU ) ’ , 4x , *m  *) 

4  format  (  2:< ,  4e20. 1 3  ,  /  ,  2x  ,  4e20. 1 3  ,  /  ,  2x ,  *  the  Initial  time  is  ’, 

♦  f  6 . 4  ) 

6  f ormat( 2 ( Ix,fl4.6>.4<2x,fl4.10>> 

12  format(9x, ’ x * , 1 2x , ’ y ' , 1 2x . ’ z ’ , 10x , ’xdot* ,9x, ’ydot* ,9x, ’zdot* . 

♦  10x,  ’ve’  .  10x,  •W  ) 

open! un 1 t*17 . f 1 1e*’ product ’ .access* ’ sequent  la  1  * , status* ’ new* ) 
open ( un 1 t* 14 . f 1 le* ’ tdata ’ .access* ’ sequential ’ , status* ’ new’ ) 

rad*  3. 14159265359d  *00/1 80. 0d  *00 
nxt*  1 
mods*  0 
n-  8 

cccccc  '  Input  initial  data 

pr 1 nt* , ’ 1 nput  the  length  of  the  flight  In  seconds,  and  time' 
read* , tp , tepoch 
t  *  tepoch 

pr 1 nt* , ’ 1 nput  state  vector,  or  have  It  calculated, y  or  n’ 
read* , typed 

if  ( typed . eq .' y ’  )  goto  5 
If  1  typed . eq .’ n ’ )  goto  7 

5  pr 1 nt* , ’ I nput  the  Inttal  state  vector  for  the  vehicle’ 
read*,y< 1 ,nxt) ,y(2,nxt) ,y(3.nxt) ,y( 4, nxt) ,y<  5, nxt) ,  y ( 6 , nxt  > . 

♦  y ( 7 , nxt ) , y ( 8 , nxt ) 

7  If  ( typed . eq .’ n ’ )  then 

pr I nt* 1 aunch  3  I te  points  are  as  follows:’ 
print*. 'ff  Petropav 1 ovsk  53. 7N  158. ZE ' 
print*,-:  Vladivostok  43. 5N  132. 0£ ' 

pr1nt*,'2  Turyantunum 
pr  1  nt* , " 3  PI esek 
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PROGRAM  TL AN . F 


Description 

This  program  accomplishes  the  numerical  integration 
check  for  the  ICBM  launch  trajectory.  The  basic  operation  is 
very  similar  to  the  operation  of  tn.f,  however,  the  program 
is  set  up  to  calculate  the  launch  point  for  the  ICBM.  of 
which  several  choices  are  available.  The  program  then 
calculates  the  initial  velocity  for  the  missile,  assumed  to 
be  staright  up  from  the  local  coordinate  system,  and  then 
proceeds  to  nudge  the  vehicle  over  so  the  gravity  turn  can  be 
executed.  The  numerical  integration  by  Haming  is  identical, 
and  instead  of  one  period  being  integrated,  the  user  can 
input  how  long,  in  seconds,  the  trajectory  is  to  be 
integrated. 


The  variables  and  usage  are  almost  identical  to  the  program 
tn.f 


ccccccccecccccccccccccccccccccccccccccccccccccccccccccccccccccceccccccec 
c  c 

cccccc  this  subroutine  calculates  the  1st  of  the  site  ccccccccccccc 

subroutine  1 st 1 me< 1 st , t . to , 1  on > 

double  precision  1st. t, to. Ion 

double  precision  thtgo.twopl.gst 

to*  0.0d  +00 

twopl-  6. 2831 853071 8d  +00 

thtgo-  33 . 8546  Id  +00* < 3 . 1 4 1 59265353d  +00/180. 0d  +00) 
gst*  thtgo  '+  1.0027379093d  +00* < ! t* 1 3. 44686457d+00/ 

♦  1 440. 0d  +00)  -  to) 

gst*  dmod ( gst. twop I ) 

1st”  gst  +  Ion 
1st”  dmod ( 1 st . twop 1 ) 

retur n 
end 

c  that  should  do  it! ! ! ! ! ! ! !  1 ! ! !  1 1 ! ! 

c 

c ccccc cccccccc cccccc ccccccccccccccccccccccccccc ccccccccccccc cccccc cccccc 


f nc 1 ude  Ven/en84d/dva1 lado/dhamlng 
I nc 1 ude  ' /en/en84d/dva 1 1 ado/r hstru ' 


If  (  rhov«(  1  >  .  eq  .0.0d+00)  then 

if  (rhovec{2>  . gt . 0.0d+00 )  az"  90.0d+00*rad 
if  !rhovec(2> . lt.0.0d+00>  az*  270.0d+00*rad 
if  ( r hovac ( 2 > . eq • 0 • 0d*00 )  than 
iz-  0.0d +00 

If  (phovac(3) .gt.0.0d*00>  a  1-  90. 0d  +00*r ad 
if  < rhovecl 3 ) . 1 1 . 0.0d*00 >  el-  -90.0d+00*rad 
and  If 

end  1  f 

if  ( ( r hovac ( 1 > . ne.0.0d»00) .and . < rhovael 2 ) . ne.0 .0d*00> >  than 
az*  datan< r hovec< 2 ) /r hovec ( 1 > > 

al"  datani phovac< 3 > /dsqrt ( rhovecl 1 > *rhovae ( 1 )  +  rhovec<2>* 

♦  rhovaet 2 ) ) ) 

tf  ( r hovac < i ) . lt.0.0d+00)  az*  az  *  180. 0d  ♦00*r ad 

If  ( (rhovecl 1 ) . gt .0 . 0d*00 ) .and. <rhovac(2) . lt.0.0d+00> >  az* 

♦  az  *  360.0d*00*rad 
and  If 

raturn 

end 

cccccccccccccccccccccccccccccccccccccccccccccccccccc cccccc cccccccccccccc 
c  c 

c  this  subroutine  calculatas  tha  position  vector  of  the  site  c 

cccccc  for  either  a  land  or  space  based  system  ccccc 

subrout  I na  radstl r s , 1  at , 1 st , t , to , ans  > 

double  precision  rs(0:3> , lat, 1st, t. to 
character  ans 

double  precision  sta . ste , st I , stomga  ,  star gp . stv (0:3) , stm , stn , 

♦  stnuo 
integer  1  no 

cccccc  land  based  sensor 

if  lans.eq.’l’)  than 
If  ( Ino.eq .0)  then 

pr Int* ,* input  tha  elevation  of  the  site* 
read*. rs<0) 
l  no*  10 
end  if 

r s ( 1 ) *  rsi 0)*dcos < lat ) *dcos< 1  St ) 
rs(2)“  r s< 0)*dcos < lat ) *ds I n< 1 st > 
rs(3)"  rs< 0) *ds I n< 1  at > 
return 
end  If 

cccccc  3pacs  based  sensor 

if  I ans.eq. ’s')  then 
If  (I no.eq .0)  then 

pr Int' Input  the  tracking  sat  orbit  data,  a  e  l.w.w’ 
read* . sta , ste . st I , stomga , star gp 
end  1  f 

stn"  dsqrt I  1 / ( sta*sta*sta  )  ) 
stm"  stn*<t-to) 

ca 1 1  randvl sta . ste, st I , stomga , s tar gp .stnuo, stm, r s , stv  > 

call  maglrsl 
tf  (lno.eq.0)  then 

64  f or mat  <  3x , *a ' . 6x , *e ' , Sx , ’ I ’ , 5x , 'omega ’ , 3x , ’argp ’ , 4x . ’m '  ) 

66  format ( 61 1 x , f 6 . 3 > ) 

write! 17,*) 

write! 17,*)  'the  tracking  satellite  data  Is' 
vrltai 17,64) 

wr 1  tel  17,66)  sta , ste , st I , stomga , star gp , stm 
Ino"  20 
end  If 
end  I  f 
return 
end 
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cccceccccccecccct 

cccccc  this  tubroutins  calculates  rho  at  and  el  for  a  given  r  and  v 
subroutine  razel < r . v . r ho . az , e 1 , to , t , r s , trm ) 


c 

ccc 


double  precision  r (0: 3 >, v<0: 3 ), rho. az ,el , to. t . rs(0s3 >. trmi 3 , 3 ) 

doub 1 e  precision  1  at . Ion , 1 st , xvec( 0s  3 ) , s vec (0:3) , evec (0t 3 ) , rad , 
r  hovel  0(3 ) . kvec (0s  3  > . r ho vec (0s 3),re(0:3),rse(0:3) 

I nteger  ing.  Inh.  Ini  ,  InJ,  Ink,  Ini  .  Inin,  Inn 
character  ans 


if  (lng.eq.0)  then 

or  I  nt* , * on ter  sensor  type,  land  or  space.  In  quotas’ 
read* , ans 
Ing-  10 

rad-  3.141 59265359d  *00/ 1 80 .0d+00 
kvec ( 1 )-  0.0d*00 
kvec ( 2 ) -  0.0d*00 
kvec(3>-  1.0d+00 
end  If 

if  1  Ians. eq. ’ 1 ’> .and. 1 Inh. eq.0> )  then 

or  Int*.  ’  Input  the  lat  and  Ion  of  site  In  deg,  east-*',  west-’ 
read*, iat. Ion 
lat-  lat*rad 
Ion-  1on"rad 
Inh-  10 
end  I  f 

call  1 st lme( 1 st , t ,to, Ion ) 
call  radstlrs, lat, 1st, t, to, ans  ) 

do  100  In  1-1 ,3 

rhove(fnl)-  r(lnl>  -  rsMnl) 

100  continue 

cal  1  mag Ir hove) 
rho-  rhov«(0) 

ccccc  set  up  local  coordinate  system 
do  110  fnj-  1,3 

zvec<lnj>-  rs( InJ )/rs<0> 

110  continue 

call  cross(kvec,zvec,evec) 
do  1 12  1nm-l ,3 

evec ( Inn)-  evec  < I nm 1 /evec ( 0 1 
1 1 2  cont l nue 

call  cross(evec,zvec,svec) 
do  114  Inn-  1 ,3 

svec(lnn)-  svecl I nn ) /svec ( 0 > 

1 1 4  cont  f  nue 

cccccc  Set  up  the  transformation  for  IJK  -  trm  SEZ 

do  120  lnl-1.3 

trm(lnl.l)-  svec(lnl) 
trm(lnl.2)»  evec(lnl) 
trial  I  r»l  .  3  >-  zvecllnl) 
cont  <  nue 
do  121  Inn  1-1, 3 

re( Innl > -  r 1  I  nn 1 ) 
rsellnnll-  rsllnnl) 
cont I nue 

Convert  to  SEZ  for  calculations 
do  130  Ink-  1.3 

rhovec(lnk)-  rhovel 1 > *trm< 1 , 1 nk  )  ♦  rhovel 2 ) *trm( 2 , 1 nk  ) 

-  rhovel 3 > *trm( 3 , Ink ) 
ccccccc  NOTE!!!!!!!  here  we  do  NOT  transform  r  to  SEZ 

ccccccc  since  we  will  not  be  calculating  H  as  In  obser  Ml! 

relink)-  reel  1 >*trm( l , Ink )  ♦  rse(2)*trm(2, Ink) 

♦  *  r se( 3)*trm<3, Ink) 

130  continue 


120 

121 

ccccc 
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print  out  final  data 

print*. 'the  final  va 1 ues  for  r  and  v  are*’ 
print*, '  r  *  * ,  y  ( 1 ,nxt> , y( 2 . nxt ) . y ! 3 , nxt ) 
print*.  ’  v*  ’ ,  y ( 4 , nxt  > ,y(5.nxt) , y <  6 , nxt  > 


(y<  inf.nxt) , i  nf  » 1 .6) 


wr ite! 1 7 .2  > 
wr ite( 17.4) 
write! 17.*) 
write! 17. *) 
end  i  f 

end  i  f 

10  continue 

endf 1 1e( un it* 17 ) 
endf  t 1  a ( un 1 1* 1 4  ) 
end 

ccccccecccccccccccccccccccccccccccccccccecceccccceecceecccccccccceeccece 
c  c 

cccccc  this  subroutine  calculates  r  and  vgiven  the  orbit  elements  cccc 
subroutine  rsndvla.a. Inc, omega .argp.nuo.m.r.v) 

double  precision  a  , e .  I  nc  .  omega  .  ar gp  ,  nuo  ,  m.  r  ( 0:  3  >  ,  v 1 0 :  3  ) 

double  precision  rad.p.el ,e0,mo 

rad*  3.14 1 59Z6535d  *00/180. 0d  *00 

no*  m*rad 

lnc«  lnc*rad 

argp*  argp"rad 

omega*  omega*rad 

p*  a*( l-e*e) 

cccccc  newton  rhapson  Iteration 
el*  mo 

S  e0»  el 

el*s0-<e0-e*dsin(e0)-mo>/( 1 .0d  *00  -  e*dcos(e0)) 
if  ! dabs ! el -e0) . gt . 1 . 0d  -12)  then 

el*-a0-(e0-e*dstn!e0)-mo)/(  1  .0d  *00  -  e*dcos(e0)) 
pr I nt*. ’ el* ' , el 
go  to  8 
end  if 

cccccc  find  the  value  of  the  true  anomaly 

nuo-  datan2< Cdsqrtl I .0d  *00  -e*e ) ) »ds In < el > / C 1 -0d  *00  -e* 

♦  dcos! el ) ) , ( e-dcos ( el ) > / ( e'dcos ( el ) -  1 .0d  *00>> 

cccccc  position  and  velocity  vectors 

r(l)»  p*dcos(nuo)/! 1 .0d  *00  ♦  e*dcos!nuo>) 

r(2>*  r ( 1 >*dtanlnuo) 

r ! 3 ) -  0.0d  *00 

v(l)“  -ds  1  n I nuo ) /dsqrt! p ) 

v<2)*  I -a*dcos  I  nuo )  ) /dsqrt  (p  ) 

v  (  3 ) -  0.0d  *00 

return 

end 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

c 

cccccc 


ccccccccc 


this  subroutine  calculates  the  magnitude  of  a  vector 
subroutine  mag(rx) 

double  precision  rx!0: 3) 

rx( 0)*  dsqrt! rx< 1 )*rx( 1 )*rx(2)*rx!2)*rx(3)*rx(3> ) 

retur n 

end 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 
c  c 

cccccc  this  subroutine  calculates  the  cross  product  of  2  vectors  cccc 
subroutine  cr oss ( r I n , v i n , vx ) 

douole  precision  r In <0i 3 ) , v I n( 0: 3 ) , vx (0 i 3 > 

vx( 1 >*  rln!2)*vln(3)-vln(2)*rin<3) 
vx( 2)*  -rln! 1 )*vln(3)*vln( 1 )*rln!3) 
vx(3>*  rln(l)*vln(2)-vin(l)*rln!2) 
ca 1 1  magi vx  ) 
retur n 


t. 


•  1 
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cccccc  calculate  Initial  angular  momentum  and  specific  mech  energy 


ca  1 1  mag ( r  ) 
ca  1 1  mag ( v  > 

angji*  dsqrt(  a*!  1  -e*e )  > 

se-  v!0>  *v!0)  !Z  .0d  +00  -  1  .0d  +00/r<0) 

tp-  tp*13.4i6864S7d  +00 

cccccc  write  Initial  header  data 

write! 17,12) 

wr 1 te( 1 7 , 6  >a . e, I nc , omega .argp , nuo , mo. tp , Itn 

write! 17,5) 

wr 1 te! 1 7 , 7 ) se , angm 

write! 17,2) 

1  na-  0 
I  nb-  0 

dsesd-  38888. d+00 
slgrho-  .0000 ld+00 
slgaz-  .001 d+00 
slgel-  .001d+00 
type-  ’  n’ 

cccccc  Initialize  hamlng  and  reset  the  time 

ca 1 1  haml ng! nxt) 
t-  0.0d+00 

wr 1 tei 17,4)  ! y ( 1 nf , nxt > , I nf - 1 , 6 > 

cccccc  begin  loop  to  Integrate 

do  10  I ncc-  0,nlt 

cal  1  hami ng! nxt ) 

1nb»  Inb  +  1 
do  30  tnd-1,3 

r!lnd>-  y!lnd,nxt) 
v!1nd)-  y!lnd+3,nxt) 

30  continue 

cal  1  razellr.v.rho.az.el ,to,t,rs.trm> 

If  (1oh.ne.10)  then 

print*, 'do  you  want  noisy  data.y  or  n’ 
read" , type 

If  Itype.eq. 'y ' >  then 

pr 1 nt* , ' I nput  a  seed  number  from  1 -2 1 *B3S47d+00’ 
r  ead* . dseed 
eno  If 
loh-  10 
end  l  f 

If  ! type. eq . ' y * >  then 

rho-  rho  ♦  ggnqf < dseed  )  *s 1 gr ho 
az-  tz  +  ggnqf ( dseed ) *s I gaz 
el-  el  +  ggnqf I dseed ) *s I  gel 
end  If 

write! 14.*)  rho.az.el.t 
If  ( ! 1 tn . ne. 50) . and . ! 1 nb . eq . 10) )  then 
Ina-  Ina  ♦  1 
If  ! ina.eq. 10)  then 
cal  1  mag! r ) 
call  mag!v) 

se-  v 10 1 *y ! 0 ) /2 . 0d  +00  -  1.0d  +00/r(0) 
call  cross! r , v , rev ) 
engm-  rcv!0) 
write! 17,7) se.angm 
i  na-  0 

print", 'the  rho  az  el  time  Is' , rho. az/rad, el/rad 
end  I  f 
I  nb-0 

If  < i ncc . 1 t . n 1 t- 10)  then 

wr1te(17,4)  ! y < I ne , nxt ) , 1 ne- 1 . 6 ) 

end  If 

If  ( inee.aa.Blt-l 1  th»n 
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cccccccccccccccccecccccccccccccccccceccccccccccccccccccccccccccccccccccc 

r 

c  This  program  checks  out  the  numerical  integrator  for 

c  hamlng.  It  does  this  by  an  integration  of  an  orbit,  once 

c  around  the  orbit. 

c  Capt.  Dave  Vallado  1984 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 
cccccc  the  common  terms 

common  /ham/  t , y ( 72 . 4 ) , f ( 72 , 4 ) , er r < 72 ) . n , dt . mode 
double  precision  t , y , f ,err ,dt 
Integer  n.nxt.mode 

cccccc  all  the  other  variables 

1 nteger  nit, 1 na . Inb . Incc , Ind , Ine, Inf 

double  precision  a , e , 1 nc .omega . ar gp ,m , tml 1 , tml 2 , tm2 1 , tm22 , 

♦  tra3 1 , tm32 . rad , se, angm , tp , r ho , as ,el . to , dseed , s Igr ho, 

♦  r (0i 3 ) , v! 0: 3 ) . r s ( 0: 3 ) , r cv I0>  3  > , trm! 3 , 3 ) , s Igel , s  fgaz 
character  type 

cccccc  begin  the  program 

2  for mat ( 9x,’x'.14x,*y',14x,'z’,12x,* xdot  * , 12x , ' ydot ’ , 1 2x , * zdot  * ) 

4  format! S(lx,fl4.11>) 

5  format! ’the  specific  mech  energy  and  ang  momentum  are’) 

6  format! 7 ( Ix,f6.3),lx.f8.3,lx, 16) 

7  format! 2 ( 8x ,f 1 4 . 1 1  )  ) 

1 2  format! 3x,’a’.6x,’e’,6x,*1’,5x, ‘omega ’,3x, *argp‘,4x, ’nuo’ ,4x, ‘m’ 

♦  ,  4x  , ’per  lod  ’  ,  3x  , '•#  1t’> 

open! un  tt-17 . f 1 1e«  ’ product  * , access* ‘ sequent  la  1 ’ , status- ’ new’ > 
open ! un 1 1-1 4 . f  f 1 e- ' tdata ' .access- ’ sequent  1  a  1 ’ . status- ’ new’ ) 

pr 1 nt* , ' 1 nput  the  data  a,e,1,w.w,m  for  the  orbit’ 

read*,a.e. 1 nc. omega. argp.m 

cal  1  randvla.e, I nc , omega , argp , nuo.m.r , v  > 

cccc  convert  from  PQW  to  IJK 

rad-  3.1 4 159265359d  +00/180. 0d  *00 
nxt- 1 

t-  8.0d  *00 
to-  0.0d  +00 
mods-  0 
n-  6 

tml 1 -  d cos  Iomega ) *dcosl argp ) -ds I n! omega  >*dslnlargp>*dcosl Inc) 
tml 2-  -dcosl omega >  *ds 1 n I argp  > -ds I nl omega >  *dcos ( argp ) *dcos !  Inc) 
tm2 1 -  d3 1 nl omega  >  *dcos ( argp ) +dcosl omega >  *ds  1  n<  argp ) *dcos!  Inc) 
tm2  2-  -ds 1 n I  omega ) *ds 1 n <  ar gp ) +dcos! omega  >*dcos!argp)*dcosl Inc) 
tm31*  d3 1 n! ar gp ) *ds f n ( Inc) 
tm32-  dcoslargp )*dsln( Inc) 

yll.nxt)-  tml i*r { 1 ) +tml 2*r I  2 ) 
y! 2 , nxt >-  tm21*r! 1 )+tm22*r!2> 
y <  3 , nxt  > -  tm3 1 *r 1 1  ) +tm32  *r 1 2 ) 
y!4,nxt>-  tml 1 *v! 1 ) +tml 2*v! 2 > 
y!S,nxt>-  tm2 1 *vl 1 ) +tm22*v< 2 ) 
y!6,nxt>-  tmC 1 *v ! 1 ) +tm32*v < 2 ) 

print*. 'the  Initial  value  for  n  and  v  •’ 
pr 1 nt*, ’ r- ’ ,yl 1 , nxt ) ,yl 2 . nxt ) ,y( 3 .nxt ) 
print*. ’ v- ' , y ( 4, nxt ) , yl 5 , nxt ) .yIS.nxt) 

cccccc  start  program 

tp-  2 . 0d  +00*3. 14l5926S3Sd  +00*sqrt( a*a*a ) 
pr  1  nt* , ’ 1 nput  the  number  of  Iterations' 
read* , n 1 t 
dt*tp/n 1 t 
nxt-0 


This  subroutine  calculates  the  position  and  velocity 
vectors  given  the  initial  orbit  data.  The  algorithm  uses  the 
eccentric  anomaly  calculation,  and  the  Newton  Rhapson 
iteration  to  find  the  mean  anomaly.  Transformations  from  the 
Perifocal  coordinate  system  are  used  to  convert  the  result 
into  the  IJK  frame. 

MAS 


This  subroutine  simply  calculates  the  magnitude  of  a 
vector. 

CROSS 

This  subroutine  calculates  the  cross  product  of  2 
ve  ct  or s . 

KAZEL 

This  subroutine  calculates  the  range,  azimuth  and 
elevation  for  the  the  truth  model  and  1 e a s t squar e s ,  and  Bayes 
filter  programs.  It  uses  the  next  2  subroutines  that  are 
described  to  accomplish  this.  Note  that  2  seperate  versions 
are  used,  one  shown  with  the  numerical  integrator  tn.f,  and 
the  other  shown  with  obser.  The  difference  here  is  the 
inclusion  of  a  transformation  to  IJK,  which  is  documented  in 
the  subroutine. 

RADST 


This  subroutine  calculates  the  position  vector  of  the 
site.  For  a  land  based  site,  the  user  is  ashed  to  input 
latitude  and  longitude  in  degrees.  The  elevation  is  input  in 
DD's.  If  the  site  is  a  satellote,  the  user  is  ashed  to  input 
the  orbit  parameters  for  the  calculation  of  the  orbit.  Notice 
that  for  any  follow  on  effort,  it  would  be  advisabler  to 
incorporate  a  seperate  time  to  the  satellite  observer  so  that 
the  satellite  could  be  positioned  over  the  launch  point.  The 
program,  as  written  now  will  place  the  satellite  at  the  same 
local  sidereal  time,  no  matter  what  orbit  parameters  are 
used. 

LSTIME 


This  subroutine  calculates  the  local  sidereal  time  for 
the  site.  Notice  that  the  value  is  input  in  degrees  for  1984. 


( 


# 


t 


the  haming  common 
rad  radians  to 

dseed  input  it  to 

sigrho  range 

sigaz  azimuth 

sigel  elevation 


degrees  conversion 

IMSL  routine  for  random  numbers 

standard  deviation 


Remaining  Variables 


r(0:3)  Position  vector 

v(0:3)  Velocity  vector 

tmll,  ...  Transformation  matrix  from  PQW  to  IJK 

se  Specific  Mechanical  Energy 

angm  Angular  Momentum 

tp  Time  period  (TD’s) 

rho  Range 

az  Az imuth 

el  Elevation 

rev (3)  r  cross  v 

trm(3.3)  Transformation  from  SEZ  to  IJK 

rad  degree  to  radian  conversion 

tQ  initial  time 

Counters  and  misc  Holders 
ina,  inb ,  incc,  ind.  ine ,  inf,  ioh 


Subroutines  used 

r  andv 
mag 
cross 
r  az  el 
r  ads  t 
1st  ime 

dhaming  (See  Appendix  B) 
rhstru  (See  Appendix  C) 

Notes 

mode  =  0  so  EOM  only 

time  step  is  critical  for  obtaining  convergence  on  the 
orb  i  t 

starting  the  integration  at  perigee  is  difficult  since 
the  vehicle  is  moving  the  fastest. 


\ 
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APPENDIX  A 


PROGRAM  TN.F 


Description 

This  program  checks  out  the  numerical  integrator  that  is 
programmed  in  Haming.  It  accomplishes  this  by  integrating  the 
two-body  equation,  once  around  it's  orbit. 

The  subroutine  randv,  takes  input  orbit  elements  and 
converts  them  to  position  and  velocity  vectors  in  the  PQV 
system  (reference  1).  A  Newton  Rhaphson  iteration  is 
emplopyed  to  convert  the  mean  anomaly  to  the  eccentric 
anomaly.  This  is  then  input  to  find  the  true  anomaly,  from 
which  the  postion  and  velocity  vectors  are  readily  obtained. 


The  main  program  then  rotates  the  postion  and  velocity 
vectors  to  the  IJK  frame  and  assigns  these  values  to  the 
state  vector  y.  The  program  then  calculates  the  period  of 
the  orbit  and  divides  the  iteration  as  l/500th  of  the  period. 
Haming  is  then  called  to  iontegrate  the  orbit  around,  with 
the  only  stops  being  to  caluculate  the  specific  mechanical 
energy  and  the  angular  momentum  from  the  position  and 
velocity  vectors  at  10  step  intervals.  The  main  data  is 
placed  in  the  system  file  with  only  minimal  input  directed  to 
the  screen. 


User  Inputs 


a 

e 

i 

Ome  g  a 
Argp 


Semi  Major  axis  (DU) 
eccentricity 
inclination  (deg) 

Longitude  of  ascending  node  (deg) 
Argument  of  Perigee  (deg) 

Mean  anomaly  (deg) 
number  of  iterations 
Land  or  space  based  sensor 

Type,  whether  or  not  you  want  noisy  data 


If  land  based 

Lat  Lat  of  the  site  (deg) 

Lon  Longitude  of  the  site  (deg) 

rs(0)  Elevation  of  the  site  (DU) 

If  space  based 

sta  tracker  semi  major  axis  (DU) 

ste  tracker  eccentricity 

sti  tracker  inclination  (deg) 

stomga  tracker  long,  of  ascending  node  (deg) 

stargp  tracker  argument  of  perigee  (deg) 

Variables  To  be  Set 
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APPBNDIX  B 


SUBROUTINE  HANIN6 

Description 

This  program  is  a  fourth  order  differential  equations 
integrator.  It  carries  four  copies  of  the  state  vector 
along,  and  extrapolates  them  to  find  the  next  value.  It  then 
corrects  this  answer  to  find  the  new  value  of  the  state 
vector . 

To  use  the  predictor-corrector  algorithm,  an  initial 
state  vector  must  be  stored  in  y(*,l).  nxt  is  then  set  to  0 
and  one  call  is  made  to  haming  to  initialize  the  EOM  EOV  etc. 
The  time  is  then  reset  to  the  epoch,  and  normal  use  can 
proceed,  as  long  as  nxt  does  not  still  =  0  upon  exit  from 
haming.  This  would  mean  that  haming  was  unable  to  use  the 
initial  state  vector  as  a  guess,  (maybe  the  step  size  is  too 
big) 

User  Inputs 
none 

Variables  to  be  Set 

t  independent  variable  time 

dt  time  step 

y(*,l)  1  copy  of  the  state  vector  to  be  input 

n  number  of  equations  to  be  integrated 

errest  estimate  of  truncation  error  (generally  not 

used) 

mode  0  -  EOM  only 

1  -  EOM  and  EOV 

nxt  sets  the  transitions  between  Haming  and  Rhs 

(  Collectively  I  call  these  variables  The  Haming  Common  ) 

Remaining  Variables 

f(*,4)  4  copies  of  the  equations  of  motion.  Rhs 

updates 

these  on  each  call  from  haming. 
tol  a  tolerance  parameter 

hh  step  size  holder 

xo  time  holder 

Counters  and  misc  holders, 

ida,  idb,  idc,  idd,  ide,  idf,  idg,  idh,  idi,  idj  ,  idk,  idl,  idn 


occccccccceccceccccccccccccccccccccccccceccccccccccccccecccccccececccccc 
subroutine  hamingtnxt1 

c  Hamlng  Is  an  ordinary  differential  equations  integrator 

c  that  is  a  fourth  order  pred 1 ctor-cor rector  algorithm 

c  which  means  that  It  carries  along  the  last  4 

c  values  of  the  state  vector,  and  extrapolates  these 

c  values  to  obtain  the  next  value  (the  prediction  part) 

c  and  then  corrects  the  extrapolated  value  to  find  a 

c  new  value  for  the  state  vector, 

c 

c  The  value  nxt  specifies  which  of  the  4  values 

c  of  the  state  vector  Is  the  ‘next"  one.  and  It  Is 

c  updated  automatically.  To  use,  an  Initial  state 

c  vector  must  be  stored  In  y<*,l).  nxt  Is  then  set  to  0 

c  and  one  call  Is  made  to  hamlng  to  Initialize  the 

c  COM  and  EOV,  etc.  The  time  Is  then  reset  to  the  Initial 

c  time  and  normal  use  can  begin  as  long  as  nxt  not  0. 

c  nxt  •  0  means  that  hamlng  was  unable  to  converge, 

c 

c  The  user  supplies  the  external  routine  rhs(nxt)  which 

c  evaluates  the  equations  of  motion. 

cccccecccccccccccccccccccccccccccccccccccecccccccccccccccccccccccccccccc 

c  Common  terms  and  variable  declarations 

common  /ham/  t . y< 72 , 4 > , f < 72 , 4 ) , er r est ( 72 ) , n . dt , mode 
double  precision  t,y ,f .errest.dt 
Integer  n, mode, nxt 

I nteger  Ida , Idb , ide . Idd , Ide, Id f , Idg , idh , Id  1 , Id J , tdk , Id  1 , 1dm, Idn 
double  precision  tol.hh.xo 

c  the  variables  are  used  as  follows 

c  t  independent  variable  (time) 

c  y ( 72 , 4 )  state  vector  in  4  copies,  nxt  points  to  next  one 

c  f <  72 , 4  )  equations  of  motion,  4  copies 

c  call  rhs(nxt)  updates  entry  In  f 

c  errest  estimate  of  truncation  error 

c  n  number  of  equations  being  Integrated 

c  dt  time  step 

c  mode  0  for  EOM.  I  for  EOM  and  EOV 

to 1  -  1.0d-12 
If(nxt)  190.10.200 

c  switch  on  stratlng  algorithm  or  normal  propagation 

c  this  is  hamlngs  starting  a  Igor  I thm . . . . a  predictor  -  corrector 

c  needs  4  values  of  the  state  vector,  and  you  only  have  1,  the 

c  I.C.  Hamlng  uses  a  prlcard  Iteration  (slow  and  painfull)  to  get 

c  the  other  3. 

c  If  It  falls,  nxt  will  be  0  on  exit,  otherwise.  nxt*l,  and  It's  ok. 

10  xo  -  t 

hh  ■  dt/2.0d*00 
cal  1  rhsl  1  ) 
do  40  Ida  -  2.4 
t  ■  t  +  hh 
do  20  idb  «  1 , n 

20  y< Idb. Ida)  -  y(1db,ida-l)  ♦  hh*f C  Idb  ,  Ida- 1  ) 

cal  1  rhs( Ida ) 
t  ■  t  ♦  hh 
do  30  Ide  *  l,n 

30  y(ldc.ida)  *  v(1dc, Ida-1)  +•  dt"f  I  1  dc  ,  Ida  ) 

40  call  rhsl Ida ) 

Idd  •  -10 
50  Ide  »  1 

do  120  Idf  •  l.n 

hh  -  y(idf.l)  *  dt*<  9 .0d+00*f < Idf . I )  *  1 9.0d+00*f ( Idf , 2 > 

♦  -  5.0d-»00*f( idf ,3)  ♦  f < Idf ,4) )/24.0d*00 

If  (dabs(hh-y< Idf .2) ) . It.tol )  goto  70 
i  de  *  0 
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y(  idf ,2)  «  hh 

hh  »  y( idf . 1 >+dt*< f < idf . 1 >  +  4 .0d+00*f < Idf ,2 >  *f ( Idf .3) > /3 .0d+00 
if  (dabs(hh-y( idf .3) > . lt.tol >  goto  90 
id®  -  0 
90  y( idf ,3)  -  hh 

hh-  y ( i df . 1 )  +  dt"(3.0d+00»f < idf , 1 !  *  9 .0d+00*f ( i df , 2 >  + 

+  9.0d+00*f ( idf .3)  +  3.0d+00*f( idf .4) )/8.0d+00 

if  < dabs( hh-y ( idf . 4 ) > . 1 t . tol )  goto  110 
id®  •  0 

'.10  y <  idf  ,4)  -  hh 

120  continue 

t  »  xo 

do  130  idg  “  2.4 
t  «  t  ♦  dt 
130  cal  1  rhs( idg) 

If  (Ida)  140,140.150 
140  tdd  -  fdd  +  1 

if  (tdd)  50.280,280 
150  t  »  xo 
id®  ■  1 

tdd  •  1 

do  160  fdh  ■  l.n 
160  errest(idh)  -  0.0 

nxt  *  1 

go  to  280 

190  Idd  -  2 

nxt  •  tabs(nxt) 

cceccc  this  is  hamings  normal  propagation  loop  - 

200  t  -  t  ♦  dt 

idl  -  mod(nxt.4)  +■  1 
go  to  (210,230) . id® 

cccccc  permute  th®  index  nxt  modulo  4 

210  go  to  (270.270.270,220) , nxt 

220  id®  -  2 

230  1 d I  «  mod ( idl  .4)  ♦  l 

1  d J  »  mod( id  1  .  4  )  ♦  1 
1 dk  «  mod ( 1 d J .  4  )  ♦  1 

cccccc  this  is  th®  predictor  part 

do  240  idm  -  l.n 

f ( I dm , 1 d 1 ) -  y( idm. idl)  +  4 .0d-00*dt*< 2 ,0d*00*f ( idm . idk ) - 

♦  f(idm.idj)  -  2.0d*00*f ( (dm, idi ) )/3.0d+00 

240  y(  Idm, Idl)  -  f(ldm.ldi)  -  0 . 92561 9335d+00*er rest ( 1  dm ) 

c  now  the  corrector  -  fix  up  the  extrapolated  state 

c  based  on  the  better  value  of  th®  equations  of  motion 

ca 1 1  r  hs (  Idl > 
do  250  I dn  -  l.n 

y< Idn , Idl )-  (9.0d*00*y( (dn, ldk)-y( Idn, idl ) +3 .0d+00*dt*( 

*  f (  idn ,  idl )  ♦  2.0d*00*f ( idn. idk )  -  f(ldn,idJ)))/8 . 0d+00 
errest(idn)  -  f(1dn,tdt)  -  y( Idn. Idl) 

-50  y( Idn, Idl)  -  y( Idn, idl)  *  0.0743801 653d+00  *  erre st(ldn) 

go  to  (260.270) , idd 
260  call  rhs ( I d  1  ) 

270  nxt  »  Idl 
280  return 
end 


APPENDIX  C 


SUBROUTINE  RHS 

Description 

This  program  calculates  the  equations  of  motion  and 
equation  of  variation  (the  A  matrix)  for  the  problem  which 
is  evaluated.  It  serves  merely  as  a  data  source  for  Haming 
and  is  called  using  nxt,  thus  no  specific  inputs  are  needed 
other  than  those  in  the  common  block  with  Haming. 

User  Inputs 

type  of  vehicle  parameters 


'0  ' 

Satellite  in  orbit 

'1 ' 

Titan  IIIB 

'2  ' 

Titan  HID 

*3  ' 

Thor  LV-2F 

Variables  to  be  set 

The  Haming  common 
Remaining  variables 

xmu 

r32 ,  rS2 
v3  2 
vel 
vat 
vve 
v  em 
mass 
acc 
am 

ma  s  so 
mdot 

ve 

Counters  and  misc  holders 

ira,  irb, ire,  ird,  ire, irf, irg,  irh,  iri,  irj , irk,  i i , j  j 
Subroutines  used 
/  ehd 

Notes 

3  different  versions  of  this  program  were  run. 
rhstru  contained  subroutine  vehd  and  is  listed  here 
rhsam  did  not  contain  vehd 

instead  of  the  call,  Ve=  y(7,nxt),  M=  y(8,nxt) 
printed  the  a  matrix  before  phidot=  a*phi 
rhslb  the  same  as  rhs  am  except  the  A  matrix  was  not 
printed 


Gravitational  Parameter  (1  ®®/-.n) 
a  5 

vJ 

Vehicle  velocity 

Combination  of  vel,  acc,  t  ve  and  m 

( vel oci ty) (Exhanst  velocity) 

(exhaust  velocity) (m) 

mdot/initial  mass 

Vehicle  acceleration 

A  matrix 

initial  mass 

mass  flow  rate 

Exhaust  velocity 


cccccccccccccccccccccccceccceccccccccccccccccccccccccccccccccccccccccccc 


I 


I 


» 


I 


I 


subroutine  rhs(nxt) 

:  rhs  calculates  the  equations  of  motion  and  /or  not  and  ^ 

c  the  equations  of  variation  for  the  problem  of  estimating 

c  launch  vehicle  performance  parameters. 

c  the  state  vector  Is  split  out  as 

c  y< 1-3, nxt)  are  the  x,y,z  components  of  the  postlon  vector 

c  y\ 4-6, nxt)  are  the  x.y.z  components  of  the  velocity  vector 

c  y(7-72,nxt)  Is  the  state  transition  matrix,  stored  as 

c  columns  of  phi  end  to  end  I 

cccccccccccccccccccccccccccccccccccceccccccccccccccccccccccccccccccccccc 

c  Common  terms  and  varafble  declarations 

common  /ham/  t. y ( 72 , 4 ) . f ( 72 , 4 ) ,er r { 72 ) , n . dt , mode 
double  precision  t,y,f,err,dt 
Integer  n.mode.nxt 

Integer  I ra , Irb , Ire . Ird , 1  re, Irf , I rg , 1 rh , I r 1 , I r J , 1 rk  i 

double  precision  r32, v32,ve1 , vat. vve.vem, mass. acc.r52,am(3,8) , 

*  mas so , mdot , ve 

c  this  data  statement  hardwires  the  parts  of  the 

c  a  matrix  which  are  never  changed .. .only  the  middle 

c  3  rows  change  each  time 

do  10  Ira* 1 , 8 

do  10  lrb-1 , 3 

ami Irb. Ira)*  0.0d*00  I 

10  continue 

do  2j8  Ire*  1 , 8 

do  2j8  1rd*7,8 

ami Ird. Irc)«  0.0d*00 
20  continue 

ami  1,4)-  l.0d*C0 
amt 2, 5)-  l .0d*£0 
ami  3 , 6  >  *  1 .0d*C0 

c  the  basic  function  of  rhs  Is  to  calculate  the  equations 

c  motion  Ithe  f  enrtles)  from  the  given  current  state 

c  I  stored  In  y)  end  the  time  t 

c 

c  • ****•••» *•**•- ••*•***•**••••*•*•••*»••»**»•*»****»»*» 

c 

c  EVALUATE  "HE  EQUATONS  OF  MOTION 


C  ntrmwwwwwwnmwwmwwwwwwmmmmwwwwwwwwwwwwwmwwmwwwwmwwwwwwwm 

c  reference  Bates  Meuller  &  White,  pg  10,  N  body  problem 

c  with  origin  In  sun. 

c 

c  position  dot  *  velocity  vector 

f < 1 , nxt )  *  y I  4 . nxt  > 

f:2,nxt)  *  yIS.nxt)  ■ 

f  3, nxt)  *  y I  6 . nxt )  ! 

cccccc  velocity  dot  *  gravity  accel 

r32  *  I  y ( 1 , nxt ) *y I l , nxt >  ♦  y I  2 , nxt ) *y1 2 . nxt ) 

♦  +  y13.nxt)*y<3,nxt)  )  **  1.5d+00 

vel  *  (  y ( 4 , nxt > *y 1 4 , nxt )  ♦  y I  5 , nxt > *y 1 5 . nxt > 

*  *  y!6.nxt)*y(S,nxt)  )  **  .5d*00 

v J2-  vel  **  3.0d  *00  I 


Sat  the  constants  whfch  will  be  used  In  the  A  matrix 


xnu*  1 .00  +00 
If  (1re.eq.0)  than 

pr Int* ,* Input  the  type  of  missile  to  be  evaluated’ 


print*, 


the  colces  are  as  follows' 


pr 1 nt* , 'Satell Ite  In  orbit 
print*, ’  T  tt.an  -  I  1 1 B 
print*, 'Tltan-IIID 
pr I nt* , ’ Thor-UV-2f  short  tank 
r sad* , I rf 
Ire-  1J 9 
end  If 

If  (l rf. eq.0J  then 
vve-  1 .  0d+tf0 
vsm-  1 .0d+t*0 
acc-0.0d+00 
mass- 1 .0d+00 
goto  6 
end  If 


call  vehdC ve.mdot.masso, t , Irf ) 

mass-  mdot/masso 
vve-  vel*ve 
vein-  ve*mass 
acc-  ve*mass/ ( : -mass* t) 

y <  7 , nxt ) -  ve 

y  <  8  ,  nxt ) -  mass 

f'A.nxt)  -  -  xnu  *  y(l,nxt>  /  r32  +  acc*y( 4 . nxt ) / vel 

f-5,nxt)  »  -  xnu  *  y<2,nxt)  /  r32  ♦  acc*y ( 5 , nxt ) / vel 

f-6,nxt)  ■*  -  xnu  *  y(3.nxt)  /  r32  ♦  acc*y  ( S .  nxt  > /vel 

f-7,nxt>  «  0.00+00 
fB.nxt)  *  0.00+00 

end  of  equations  of  motion 
is  this  all  7 

1f(  made  .eq.  0)  return 

It  tsnt  all  ...  calculate  the 


EGUATIONS  OF  VARIATION 


FIRST,  calculate  a  matrix....  only  lower  3x3  Isnt  hard  wired 
r 52  «  r 32  **(  £ .00+00/3 .00+00  ) 
e  diagonal  terms  In  a  matrix 

anIA.l)  -  -xmu/r32  +  3 .0d+00*xmu*y < 1 , nxt > *y ( 1 , nxt > /r 52 
an(5.2)  -  -xmu/r32  *  3 . 0d+00*xmu*y I  2 , nxt > *y ( 2 , nxt > /r 5 2 
an(6,3)  -  -xmu/r32  ♦  3 .0d+00*xmu*y ( 3 , nxt ) *y ( 3 . nxt ) /r 52 

off  diagonal  terms  In  a  matrix 

uee  symmetry  to  avoid  as  much  calculation 

as  poss  1b  1  e .  .  .  th  I  s  point  Is  deep  within  lots  of  loops!1.  !  1 


'»  ^ j  «'*V« 


\-“v 
*  .*'  ■  •  .‘1 


an ( 4 , 2 )  ■  3 .0d-00*xmu*y < 1 . nxt ) *y < 2 , nxt > /r 52 
an( 5 , 1 )  “  am(  4.2) 

an<  4 . 3 )  -  3.0d-00*xmu*y< 1 , nxt > *y < 3 , nxt ) /r 52 
am( 6 , 1  )  “  am<  4 . 3 ) 

an(S,3)  *  3  .0d-00"xmu*y(  2  ,  nxt )  *y<  3  ,  nxt. ) /r  52 
an ( 6.2)  *  am( 5.3) 

cccccc  now  same  stuff  for  the  other  terms 

am(4,4)“  -y < 4 , nxt ) *y( 4 , nxt ) *aec/ v32  +  acc/vel 
an(5,5)“  -y( 5 , nxt ) *y( 5 , nxt ) *acc/v32  *  acc/val 
an(6,6>“  -y ( 6 , nxt ) *y ( 6 , nxt ) "acc/ v32  ♦  acc/vel 

an (4,5)»  -y(4,nxt)*y(5,nxt)*acc/v32 
an( 5 , 4 ) ■  am( 4,5) 

an(4,6)“  -y(4,nxt)*y(6,nxt)*acc/v32 
an( 6 , 4  > •  am( 4,6) 

an( 5 , S ) •  -y(5,nxt)*y(6,nxt)*acc/v32 
an (6,5)“  am (5, 6) 

an(4,7)“  y(4,nxt)*acc/vve 
an(5,7)“  y ( 5 , nx t ) *acc/ vve 
an(6,7)“  y ( 6 , nxt ) "acc/vve 

vat*  acc*acc*t/ vem  +  acc/mass 

an(4,8)“  y( 4 , nxt )*vat/ vel 
an(5,8)“  y( 5 , nxt ) "vat/ vel 
an(6,8)“  y ( 6 , nxt ) "vat/ ve 1 

c  the  a  matrix  Is  now  calculated 

c 

c  NOW,  calculate  phi  dot  ■  a  "  phi  and  put  Into  last 

c  64  slots  of  f  matrix 

do  800  Irg  »  1.8 

do  800  trh  -  1.8 

1 r 1  *  G"trh  ♦  Irg 
ft.  1r1  .nxt)  *  0.00d+00 
do  700  1 r J  -  1.8 

Irk  “  8*  trh  ♦  tr J 

fi  Irl.nxt)*  f-(lrl.nxt)  *  am(  Irg  ,  1  r  J  )  *y(  I  r  k  ,  nxt ) 
700  continue 

300  continue 

cccccc  ohl  dot  ■  a  *  phi  is  now  done 

raturn 

end 


ccccecccccccccccccccccecccccccccccccccccecccecccccccccccccccccecccccccc 

c 

tills  subroutine  calculates  the  launch  vehicle  data  c 

subroutine  vehc < ve , mdot , masso , t , Irf ) 


double  precision  ve, mdot, masso, t 
Integer  Irf 

double  precision  time, Isp, thrust 

time"  t*13 . 446E6457d +00 
If  ( t  rf .eg . 1 )  then 

If  (time. It. 1 .3d+00>  then 
Up-  ZS6.0d+00 
thrust*  434300. 0d*00 
masso-  305 9 70. 0d +00 
end  l  f 

if  ( (time. lt.4.0d+00) .and. (ttme.ge. 1 .3d+00> >  then 
Up-  317  .06*00 
thrust*  102300. 0d+00 
m3Sso-“381 6 .0d+00 
end  1  f 

if  ( (time. lt.6.0d+00> .and. <t1me.ge.4.0d+00> >  then 
Up-  292 .0d+00 
thrust*  16000.0d+00 
masso-  14676. 0d+00 
end  1  f 
end  If 


If  < irf .eq.2)  then 

if  (time. It . 1 .3d+00)  then 
lap-  301.0d+00 
thrust*  523000 -0d+00 
masso-  307500 . 0d*00 
end  I  f 

If  ( (time. It. 4.0d+00> .and. (time. ge. 1 .3d+00> >  then 
Up-  3 1 7 -0d+00 
thrust-  102300. 0d+00 
masso-73670.0d+00 
end  1  f 

If  { (time. lt.6.0d+00) .and. (t tme.ge.4.0d*00> >  then 
1 3p-  444 .0d+00 
thrust*  30000. 0d+00 
masso-  -36 122 .0d+00 
end  1  f 

if  ( ( 1 1  me . 1 1 . 10 .0d+00) .and . ( t Ime . ge . 6 .0d+00 ) )  then 
Up-  2E4.0d+00 
thrust*  15000. 0d+00 
masso-  2721 .0d+00 
end  1  f 
end  if 

If  < Irf ,eq.3>  then 

if  ( 1 1  me.  1 1. .  2 . 5d+00)  then 
Up-  25 1 .0d*00 
thrust*  170000. 0d+00 
masso-  106092. 0d+00 
end  I  f 

if  ( ( t Ime. 1 t.8 .0d+00> . and. ( t Ime. ge. 2 . 5d*00> )  then 
Up-  290.0d*00 
thrust*  10000. 0d+00 
masso-  1743.7d+00 
end  if 
end  1  f 

ve-  1 sp/806 . 811874 4d +00 
mdot"  thrust/ve 


retur n 
end 

cccececccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 
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APPENDIX  D 


PROGRAM  TNA.F 

Descript  ion 

This  program  checks  out  the  A  matrix  as  described  in 
chapter  3.  The  inputs  are  simply  the  state  vector  y(*,l), 
which  can  either  be  left  unchanged,  or  re-input  by  the  user 
at  run  time.  The  program  then  calls  rhs  which  calculates  the 
A  matrix  directely  and  prints  it  out  to  a  separate  file. 
Then,  using  equation  (4-1)  the  A  matirx  is  again  calculated 
using  the  different  method.  The  results  are  printed  out  to 
the  same  output  file,  and  the  results  can  then  be  compared. 

User  inputs 

*y*  or  'n'  ans-  whether  or  not  the  user  wants  to 

change  the  given  state  vector. 

If  yes,  input  the  new  state  vector  y(8) 

Variables  to  be  set 

the  haming  common 

Remaining  variables 

xu(8,nxt)  nnpertnrbed  state  vector 

fu(8,nxt)  unperturbed  F  matrix 

amat(8,8)  A  matrix 

delta  delta  of  each  iteration 

Counters  and  misc  holders 

iaa, iab, iac, iad, iae, iaf, iag, iah 

Subroutines  used 

rhsam  (See  Appendix  C) 

Notes 

watch  using  nxt  in  and  out  of  dhaming,  it  is  not  always 
equal  to  1 . 


THE  A  MATRIX 


+  3  fix' 


3|ixy 


3  |ixz 


-vx2a  +  a 


-fi  +  3^7' 


3(iyz 


-vyvxa 


3(ixz 


3fiyz 


-|i  +  3jiz- 


"vxvza 


'xvya 


fy  »  +  a 


"vxvza 


vyvz* 


a^t  +  a 


v  'Ven  M 

li/ll*  + 1 

y  'vem  M 


fyV 


-vz2a  +  a 


If(_  +  _! 

y  Wem  M 


where 


3  Fi  /  9 


c ccccccceccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

z  This  program  checks  out  the  A  matrix  for  the  problem  of 

c  estimation  of  launch  vehicle  performance  parameters.  It 

c  does  this  by  having  rhs  calculate  the  A  matrix  from  an 

c  Input  state  vector.  Then,  each  element  of  the  3tate  vector 

c  is  perturbed,  and  the  columns  of  A  are  calculated  by 

c  subtracting  the  original  F  matrix,  from  the  perturbed 

c  F  matrix  and  dividing  by  delta, 

c 

c  Capt.  Dave  Vallado  1984 

cccecccccccccccccccccccccccccccccccccccccccccccccccccecccccccccccccccccc 

common  /ham/  t , y( 72 , 4 > , f ( 72 , 4 ) ,errest< 72 ) , n . dt ,mode 
double  precision  t.y.f ,errest,dt 
integer  n.mode.nxt 

integer  <aa,  lab',  lac,  lad,  lae,  laf ,  lag,  tah 

double  precision  xu< 8 , 1 > , f u< 8 , 8 ) , amat( 8 , 8 ) .del ta 

character  ans 

4  format(8( lx,e!2.6) ) 

open ( un 1 t9 16 ,f 1 1e9 ’ aprod ’ .access9 ’ sequential  * , status9  *  new’ ) 
nxt9 1 
mode  9 l 
n-  72 

t9  . 4967244 1 3d*00 
dt-  0.0d+00 

cccccc  initialize  the  state  vector 

y< 1 , nxt) 9  2. 4848282763d  *00 
y ( 2 , nxt ) 9  ,19950378763d  *00 
y  <  3  ,  nxt  >  9  ,19950378763d  *-00 
y ( 4 . nxt > 9  -.07 13766449 4d  *00 
y <  5 , nxt  >  9  . 4443S648672d  *00 

y<  6 . nxt ) 9  . 44435648673d  *00 

y <  7 , nxt ) 9  0.0d  *00 
y <  8 , nxt ) 9  0.0d  *00 

print*, ’the  current  y-12345678  values  are ’ , ( y 1 laa , nxt > , 

*  I  aa» 1 , 8  > 

print*, ’do  you  want  to  change?  y  or  n  In  quotes’ 
read* , ans 

if  lans.eq.’n')  goto  5 

pr In t*. ’ input  the  new  state  vector , 1  to  8, and  the  time' 
read* , <  y ( 1 ab , nxt ) , lab9 1 , 8 ) , t 

5  wr 1 t*( 16 . * ) ’ the  A  matrix  check  data  Is  as  follows  for  ’ 
wr 1t»( 16 , * ) ’the  Initial  state  vector  y  of’ 

wrltal 16. *>  <y< lac, 1 ) , lae-l ,8) ,t 

wr  1  te(  1 S  ,  *  > 

cccccc  call  rhs  and  have  the  A  matrix  printed  out 
call  r hs ( nxt ) 

cccccc  set  Initial  state  and  f  vectors 

do  8  lad9  1 .8 

xu(lad.nxt)9  y< lad, nxt) 

fullad.nxt)9  f( lad. nxt) 

3  continue 


.  •; 


ccccc  perturb  sach  element 


wr i ta< 1 6 . * ) ’ now  perturb  each  element  of  the  state  vector’ 
mode*0 

do  10  ( »a*  1 , 6 

delta*  -y  •  I ae, nxt >  *\  0001d  »00 
If  < dabs! del ta ) . 1 t • 1 .0d- 1 4 )  goto  9 
yllae.nxt)  *  delta  +  y(tae,nxt) 
ca 1 1  rhs( nxt ) 
do  9  laf *  1 , 8 

imat ( laf, I ae ) *  (f(  laf,nxt)-fu< laf, nxt) )/delta 
continue 

yltae.nxt)*  xu(lae.nxt) 

0  continue 

wr its! 16 , 4 )  (( amatl lag,tahr,tah*l,3),lag*l.S) 

andf I  1 e<  un 1 1* 1 6  ) 

end 

yee  haw! ill!!!!!!!!!!!!! 

ccceccccccccccccccccccccccccccccceccccccceccccccccccccecceccccccec ccccc 
include  '/en/en84d/dva11ado/rhsam* 


APPENDIX  E 


PROGRAM  TNPH.F 

Description 

This  program  accomplishes  much  the  same  function  as 
tna.f.  It  really  only  mechanizes  the  discussion  in  chapter 
3.  The  input  is  a  state  vector  y(8)  which  can  be  altered  by 
the  user,  however,  one  should  note  that  the  time  step  is 
based  on  an  orbit  from  the  family  where  a=2.5DU.  The  program 
calls  haming  which  numerically  integrates  the  state  through 
about  of  the  orbit.  The  program  then  prints  out  the  o 

matrix  to  a  file  called  phprod.  Then,  one  by  one,  the  states 
are  perturbed,  translated  through  time,  and  reinitialized 
until  equation  4-2  has  been  used  to  successively  calculate 
the  columns  of  the  o  matrix.  The  results  of  the  second 
calculation  are  then  output  to  the  same  file  for  comparision. 

User  inputs 

'y'  or  'n'  ans-  whether  or  not  you  want  to  change 

the  given  state  vector 
If  yes,  input  the  new  state  vector 

Variables  to  be  set 


the  haming  common 
tp 


time  period 


Remaining  Variables 

xu  (  8) 
cph i (8,8) 
xut ( 8) 
delta 


unperturbed  state  vector 
calculated  o  matrix 

unperturbed  state  vector,  moved  through  time 
delta  on  each  iteration 


Counters  and  Disc  holders 

ipa, ipb , ipc, ipd, ipe , ipf , ipg, iph, ipi, ipj , ipk, ipl, ipm, 
ipn, ipo, ipp, ipq. ipr, ips, ipt, ipu, ipv 

Subroutines  used 

dhaming  (See  Appendix  B) 
rhslb  (See  Appendix  C) 

Notes 


be  sure  to  set  n=  7  2 ,  t  =  0,  dt  = 

Remember  that  a  =  2.5  DU's  I! 

must  reset  o  and  state  on  each  iteration 

watch  values  of  nxt  in  and  out  of  the  subroutine  calls 


85 


rccccccccccccccccccecccccccccccccccccccccccccccccccccccccccccccccccc 


This  program  checks  out  the  Phi  matrix  for  the  problem  of 
estimation  of  launch  vehicle  performance  parameters.  It 
does  this  by  having  rhs  calculate  the  Phi  matrix.  Then, 
each  element  of  the  Input  state  vector  Is  perturbed,  and 
movad  through  time.  The  columns  of  the  Phi  matrix  are 
calculated  by  subtracting  the  orglnal  state,  from  the 
perturbed  state,  and  dividing  by  delta. 

Capt.  Dave  Vallado  1984 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 


common  / ham/  t , y ( 72 . 4 ) , f ( 72 , 4 ) , er r est ( 72 ) . n , dt , mode 
douale  precision  t , y , f , er rest , dt 
Integer  n.mode.nxt 

douole  precision  xu < 8 , 1 ) , cph 1 ( 8 , 3 > , xut ( 8 . I > . tp , del ta , tstar t 
I ntager  1  pa , ipb, Ipc, Ipd, Ipe, Ipf, Ipg, fph , 1 p  t , ipj, tpk , ipl , 1pm, 
♦  1pn,ipo,lpp,lpq,1pr,tps,lpt,lpu,tpv 

character  ans 

format! 3 ( lx.f 12.6) ) 

open! unit-15. fll a- 'phprod' .access"’ sequent  lal’, status-’  new’  ) 

n-72 
mod 9*1 
nxt«  1 

tstart-  .496729413d.  +00 
cc  initialize  the  state  vector 


y(l.nxt)-  2 . 484028Z7630d 
y <  2 , nxt ) ■  0.19950378763d 

y { 3 , nxt ) ■  0.19950378763d 

y ( 4 , nxt ) -  - . 07 137664494d 

y <  5 , nxt  > -  . 44435648672d 

y 1 6 , nxt ) *  . 44435648672d 

y 1 7 , nxt ) ■  0 . 0d  +00 

y ( 8 , nxt ) "  0.0 d  *00 
tp-  2.0d  +00-3. 1415962535d 


+00 
+00 
+00 
♦00 
♦00 
♦  00 


♦00*dsqrt (15. 62Sd+00  > 


pr1nt*,*the  current  values  of  y  -12345678  are’ , ( y ( 1 pcc , nxt ) , 
♦  lpcc«  1,8) 

print*, ’do  you  want  to  change  the  values?’ 
read* ,ans 

If  (ans.eq.’n’)  goto  5 


pr 1 nt* , ’ 1 nput  new  state  vector, time  and  tp  for  tp/500' 
read*, (y( 1  pa. nxt) , Ipa-l ,8) .tstart , tp 


ccc  set  unperturbed  state  vector 

do  6  1 pb" l , 8 

xu(ipb,nxt)»  y(lpb,nxt) 
cont  1  nue 


dt-tp/500.0d  +00 
t»  tstart 


ccc  Initialize  phi  matrix 
do  7  Ipc«  9,72 


y( Ipc, nxt)-  0.0 d  +00 
cant  I nue 

do  8  Ipd-  9,72.9 

y ( Ipd, nxt )-  1 .0d  +00 
cant  1 nue 


write! 15 , *  > 
wr  1  tel  15  ,  *  ) 
write! 13,*) 
write! 15,4) 


’this  data  is  from  the  state  vector  y  -’ 
(  y <  tpw, nxt ) , I pw- l , 3 ) , t 
’the  Initial  phi  matrix  is’ 
i  (y( Ipe. nxt) , Ipe-lpf ,72.8) , lpf-9. 16) 
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MICROCOPY  RESOLUTION  TEST  CHART 

NATIONAL  HURT  Alt  Of  STANDARD^  ! Oft  *  A 


ccccccc  prooogate  unperturbed  state  vector  through  time 


nxt*0 

ccccccc  initialize  homing  and  reset  time 

call  hamlng(nxt) 
t*tstart 
do  9  1pg*l,50 

cal  1  hamingl  nxt) 

If  (1pg.eq.50)  than 

print*, ’the  last  Itaratlon  of  tha  phi  matrix  is* 
print*, ( (y< (ph.nxt), iph*1pt,72,8>. ip  I  *9 , 16  > 
end  if 
contl nua 

£)  cccccc  sat  unperturbed,  moved  through  time,  state  vector 

do  10  1pJ*l,8 

xut(lpj.l)"  y(lpj.nxt) 

10  continue 

wrlte(15,*)  *phl  Is  as  follows  after  50  Iterations’ 
write! 15,4)  ( (y< Ipk.nxt) , tpk* Ip  1 , 72 , 8 > . tp 1 -9 . i 6 1 

(  cccccc  now  perturb  each  one  of  the  elements  of  the  state  vector 

do  20  1pm* 1,8 

cccccc  reset  the  state  and  phi  and  perturb  one  element 

do  21  lpr*l,8- 

y (  Ipn  ,  1 ) *  xu(  ipn. 1 ) 
continue 
do  22  ipo*  9,72 

y{ Ipo, 1 )»  0.0d  ♦00 
continue 

do  24  Ipp-  9.72,9 

y<  ipp, 1 )•  1 . 0d  *00 
cont 1 nue 
nxt*  0 
t*  tstart 

delta*  -  xu(  1pm. 1 )*.0001d  +00 
If  ( abs( delta > . It. 1 .0d  -14)  goto  20 
pr 1 nt* , ’delta*’ .delta 
yllpm.l)*  del ta+xu< 1pm, 1 > 
cal  1  haming(nxt) 
t*tstart 

cccccc  move  the  perturbed  vector  through  time,  and  cal  phi 

do  26  t pq* 1 , 50 

call  hamtng(nxt) 
cont I nue 
do  28  1pr*l,8 

cph  1  <  Ipr  ,  1pm ) ■  <y(1pr,nxt)  -  xut( Ipr , 1 ) (/delta 
cont I nue 
continue 

wrlte<15,*)  'calcu  phi  Is  as  follows  after  50  iterations’ 
wr I  tel  15 , 4)  ((cph((1pt,fps),tps*l,8),lpt*1.8> 
andf 1 le<  un It* 15 ) 
print*. ’the  calculated  phi  is’ 
print*, ( (cphi ( Ipv, 1pu> , 1pu*l ,8) , tpv*l ,8 ) 


cccccccccccccccccecccccccecceccccceccccccccccecccccccceccccecccccccccccc 

Include  ’ /en/en84d/dva 1 1 ado/dham  Ing  • 

Inc  1 ude  * /en/en84d/dva 1 1 ado/r hslb ’ 


26 

28 

20 
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APPENDIX  P 


I' 


t. 

PROGRAM  TNI.F 

c 

_L._ 

3 

Description 

This  program  checks  out  the  H  matrix  for 

the  specific 

problem.  It  does  this  by  use  of  equation  4-3 

in  chapter  3. 

The  program  starts  by  inputting  an  initial  state 

vector  which 

|« 

the  user  can 

change  at  run  time.  Then,  similar  to  the  A 

L.’ 

V 

1 

matrix  check 

done  in  tna.f,  the  program  has  obser  calculate 

► 

the  H  matrix 

directly,  and  then  calculates 

the  oolumna 

; 

individually  by  perturbing  each  element  in  the 

state  vector. 

. 

and  dividing  out  the  resulting  differences  in  the  calculated 

6  matricis. 

r  o 

- 

User  Inputs 

> 

'y'  or  ‘n’ 

ans-  whether  or  not  the  given 

state  vector 

should  be  changed 

If  yes. 

input  the  new  state  vector 

■  <- 

* 

Variables  to 

be  se  t 

1 

the  haming  common 

Remaining  Variables 

xu(  8) 

unperturbed  state  vector 

>_ 

hm( 3,8) 

H  matrix 

v 

zpredn(3)  unperturbed  6  matrix 

-  - 

zpred 

6  matrix 

V- 

del  ta 

delta  on  each  iteration 

i  • 

Counter 

and  misc  holders 

— i 

iha, ihb. 

ihc, ihd,  ihe,  ihf ,  ihg, ihh, ihi, ihj 

'  ■* 

■* 

Subroutines  used 

>■' 

obser  1 

(See  Appendix  G) 

;L 

1  r 

razel  j 

* 

randv 

- 

• 

mag 

„ 

cross 

(See  Appendix  A) 

r  adst 

1st ime 

•  - 

*! 

^ 

y.-j 

y-  * 

m  f 

- 

*■  - 

-  -W 
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• 

.  -  i* 

cccccccccccececcceeccccccccccccccccceccccccccccccccecccccccccccecccccccc 

c 

c  This  program  checks  out  the  H  matrix  for  the  problem  of 

c  estimation  of  launch  vehicle  performance  parameters.  It 

c  does  this  by  having  rhs  calculate  the  H  matrix  from  an 

c  input  state  vector.  Then,  each  element  of  the  state  vector 

c  is  perturbed,  and  the  columns  of  H  are  calculated  by 

c  subtracting  the  original  G  matrix,  from  the  perturbed 

c  G  matrix  and  dividing  by  delta, 

c 

c  Capt.  Dave  Vallado  1984 

ccccccccecceeccccccccccccceccccccceccccccecccccccccecceeccccccccccceccec 

common  /ham/  t ,y{ 72 , 4 > , f < 72 , 4 ) ,errest( 72 ) , n ,dt .mode 
double  precision  t.y.f .errest.dt 
integer  n.mode.nxt 

i nteger  nxt .mode, iha . Ihb, the, Ihd, the, i hf , thg, Ihh, ihl , Ihj 
double  precision  xu< 8 , 1 ) , zpredu! 3 ) ,hm! 3 , 8 ) , zpred ( 3 > , del ta 

♦  ,h(3,8).ql(3,3), tob 
character  ans 

4  format(8( lx,el2.6) > 

open  <  un 1 t" 1 3 , f  tie* ’ hprod * .access" ’  sequent  I  a  1 ’ , status" ’ new’ ) 
nxt"  1 

dt"  0.0d  *00 
t-  4 .917621 19d*00 
mode  " 1 

ccccec  initialize  the  state  vector 

y( 1 , nxt) *  . 80235902456d  +00 
y! 2 , nxt ) ■  1 .67424908479d  *00 
y ( 3 , nxt ) ■  1.67424908471d  *00 
y 1 4 . nxt ) *  -.59899756237d  *00 
y ( 5 , nxt ) »  . 14353034570d  *00 

y <  6 , nxt  >  *  . I43S3034570d  *00 

y <  7 , nxt ) *  0.0d  *00 
y!8,nxt>*  0.0d  *00 

print*. ‘the  current  y-12345678  values  are’ . 1 y! Iha . nxt > , 

♦  1  ha* 1 , 8 ) 

print*. ‘do  you  want  to  change?  y  or  n  In  quotes’ 
read'.ani 

if  lans.eq.’n')  goto  5 

pr 1 nt* , ’ 1 nput  the  new  state  vector, 1-8,  and  time* 
read* , ! y ( 1 hb , nxt ) , 1 hb“ 1 , 8  )  , t 

5  wr 1ts< 1 3 , * ) ’ the  H  matrix  check  data  Is  as  follows  for  ’ 
wr lta( 13, *) ‘the  Initial  state  vector  y  of’ 

write! 13 , » )  (y( Ihc, 1 > , lhc-1 .8)  ,t 
wr 1ts< 13, *> 


ccccec  call  obser  and  calculate  G  and  H 


tob«  t 

call  obser ( tob ,ql , zpred , h , nxt ) 

wrlte(13,*)  ’obser  calculates  H  as  follows  ’ 
do  7  ihl*  1,3 

write! 13,4)  ! h<  1h 1  , 1 h J > , 1 h J-l . 8 ) 

7  continue 

cccccc  set  Initial  state  and  g  vectors 

do  8  ihd"  1,8 

xullhd.nxt)"  y! Ihd, nxt) 

3  continue 

do  8  I hk» I , 3 

zpredu! Ihk)*  zpred! ihk) 

6  continue 
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ccccec  perturb  itch  element 


node»0 

do  10  I h«*  1 , 8 

delta"  -y  1he,nxt)*.0001d  +00 
y(the.nxt;  ■  delta  +  y(lhe,nxt> 
call  obser ( tob ,q 1 , zpred , H , nxt ) 

If  (aba (delta ) . It. 1 .0d  -14)  goto  10 
do  9  1hf«  1.3 

hm( (hf, 1  he  >  ■  (zpred ( Ihf )-zpredu( 1hf> ) /delta 

9  continue 
y(1he,nxt;“  xu(lhe.nxt) 

10  continue 

cccecc  Print  out  the  result 
write! 13.*) 

write! 13,*)  'the  perturbed  calculation  for  H  Is  as  follows’ 
write! 13,4)  < (hm( Ihg, Ihh) , lhh-1 .8) . Ihg-l ,3) 

wr  tte( 13,*) 

endf 1 le( un lt*l 3 ) 
end 

c  yee  haw! 1 1 lit ! Ill l !  t !  1 1 1 

cccccccccccccccccccccccccccccccceccceccccecccccccccccccecccccccccccccccc 
include  * /en/en84d/dva 1 lado/obser * 


APPENDIX  G 


SUBROUTINE  OBSER 

Description 

This  program  calculates  the  observation  relationships. 
The  main  function  is  to  calculate  the  G  matrix,  the  predicted 
data,  and  the  H  matrix  which  is  simply  the  partial  of  0  with 
respect  to  the  state  vector.  I  have  called  the  G  matrix  in 
the  derivations  by  G  and  z,  and  the  obser  program  uses  zpred 
and  z.  They  are  the  same  thing. 

User  inputs 

none 

Variables  to  be  set 

The  Earning  Common 

tob  time  of  each  observation 

trm(3,3)  transformation  from  SEZ  to  IJK 

to  initial  time 

Remaining  Variables 

ql  Q  inverse 

zpred ( 3 )  G  matrix 

h(3,8)  H  matrix 

r(3)  position  vector 

v(3)  velocity  vector 

rho  range 

az  az imuth 

el  elevation 

rs(3)  sitevector 

sigrho  range 

sigaz  azimuth  standard  deviation 

sigel  elevation 

Counters  and  misc  holders 

ioa,  ioc,  iod,  iof,  ion,  iov ,  iow,  ohml  ,  az  dnom ,  el  dnom,  ioe  , 
elbtm, hit (3 , 3 ) 

Subroutines  used 
raz  el 
randv 

mag 

cross  (See  Appendix  A) 
radst 
1 st ime 

Notes 

do  not  transform  r  to  SEZ  as  in  tn. f 


no*-  onnononn  oonn 


ccccccccecccccc cccccc ccccecccccccccccccececcccccceccccecccccccccceccceec 

subroutine  obser  1 tob  .  q  1 .  zpr  ad .  h  ,  nxt ) 

c  this  program  calculates  the  following  data 

c  zpred  observation  matrix 

h  H  matrix 

ql  q  inverse 

ececcccccccccccccccccccccccccccccccccccccccecceccecccccccccccceeeccecec 

double  precision  tob ,ql ( 3 , 3 ) , zpred! 3 ) . h( 3 . 8 ) 

Integer  nxt 

common  /ham/  t.yl 72 , 4 > ,f 1 72 , 4 > ,err < 72 ) . n ,dt . mode 
double  precision  t,y,f ,err ,dt. 

Integer  n.mode 

double  precision  to.  r  <0t  3 ) ,  v(0:  3 ) ,  rho.az  ,  el  ,rsl0>3 ) .  s  Igel  ,  loe 
+  , ohml , azdnom, eldnom , elbtm. h 1 t( 3 , 3 ) , trm( 3 , 3 ) .slgrho.slgaz 

1 nteger  I oa , 1 oc , 1 od , 1  of . lot, 1 ou . 1 ov , low 


RANGE  -  AZIMUTH  -  ELEVATION  DATA 


q  Inverse  matrix 

do  IB  loe-  1,3 

do  IB  lod-  1,3 

qllloc.todl*  B.B d  *BB 
B  continue 

print*. ’ input  the  sigma  rho,  az  el* 

read". si  hr ho. s Igaz ,s igel 

slgrho-  .BBBBld*BB 

stgaz-  .BB\d*BB 

s  Igel-  .BBld*BB 

q  1  (  1  , 1 )  -  s  Igrho'slgrho 

ql<2.2)»  slgaz'slgaz 

q  l  ( 3 . 3 ) *  stgel"s1ge1 

loe-  ql(I,l>«ql(2,2)"ql<3,3) 

do  12  1 of—  1.3 

ql(lof,1of)-  q  1  (  I  of , tof ) / loe 
12  continue 

to-  B.Bd  *00 
do  l 1  toa-1 ,3 

r 1 loa ) »  y ( ioa , nxt > 
vlloal-  y< 1oa+3 ,nxt) 

1  1  cont I nue 

cal  1  razel (r .v.rho.az.el ,to,t.rs, trm) 

ccccc  this  calculates  the  G  matrix 

zpred ( 1 ) -  rho 
zpred<2)«  az 
zpredl31-  el 

cccccc  The  H  matrix 

cccccc  note,  the  r  and  rs  are  In  SEZ 

ohml-  <r( 1 )-rsf l >)*<rt 1 )-rsll ) )  ♦  <r(2)-rs<2> )*(r(2)-rs( 2) ) 

♦  ♦  (r(3>-rs(3) )*< r(3)-rs!3) ) 

hi  1,1)  -  1  1  .W-OT/dsqrtlohml )  >*<r<  1  )-rs(  1  )  ) 
hi  1,2)  •  ( 1 .0d-00/dsqrt(ohml ) ) * ( r ( 2 ) -r s 1 2 ) > 
hi  1,3)  -  i  1  .tfd-'MT/dsqrtlohml  )  >  »<  r  <  3  ) -rs <  3  )  > 
hi  1 ,4)  -  0.0d*BB 
hi  1 .5  )  -  B.Bd*BB 
hi  1  .  6  )  -  B.0d*00 
hi  l  ,7)  ■  0.0d*00 
h<  1 .8  )  -  B.Bd+BB 
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azdnom-  1 .0d*00  *  ( (r< Z)-rs(Z) )/(r( 1 )-r*( I ) ) )* 

*  < (r< 2>-rs(2)  » / <  r { 1  )-rs< 1 >  )  ) 

h  <  2 , 1  >  »  l-(r»2)-ps<2) >/f(r< 1 >-ps( 1 ) )  *  (r( 1 )-rs( 1 ) ) ) >/azdnom 
h  (  2  <  2 )  -  (  1  .0d-00  /  (  r  <  1 ) -rs<  1 )  )  ) /azdnom 

h  >  2 , 3  )  «  0.0d  -00 

h<2,4)  -  0.0d  -00 

hi  2. 5)  -  0.0d  -00 

h ( 2 , 6  )  -  0.0d  -00 

h (  2 , 7  )  .-  0.0d  -00 

h ( 2 , 8  )  -  0.0d  -00 

elbtm-  (p(1)-ps(1)>*{p(1 )-rs( 1 ) )  *  (p<2)-ps(2) >-(r{2)-rs<2)  > 
eldnom-  1.0d*00  *  ( < p < 3 ) -rs< 3 ) ) *< r < 3 !-rs ( 3 )) > /elbtm 

H( 3 , 1 )  *  ( { - <  p  < 1 )-fs< 1) )*(r<3)-rs(3) ) )/dsqrt<elbtm*elbtm*elbtm>  > 

*  f  eldnom 

h  (  3 , 2  )  -  (  (-<r(2)-ps<2)  )*(r(3)-rs<3)  )  )/dsqrt(elbtm*elbtm*elbtm)  ) 

*  /  eldnom 

h(3.3)  -  ( 1 .0d-00  /  dsqrt(elbtm) )  /  eldnom 
h ( 3 , 4  )  -  0.0d  -00 
h  (  3 , 5  )  *  0.0d  -00 
h ( 3 , 6  )  -  0 . 0d  -00 
h ( 3 ■ 7  )  -  0.0d  -00 
h <  3 . 8  >  -  0.0d  -00 

ccecc  Convert  to  IJK 

do  2010  tot- 1.3 

htt< 1 . tot)-  h( 1 . tot) 
h 1 1<  2 . tot)-  h( 2 , tot) 
h 1 t( 3 , tot)-  h( 3 . tot) 

2010  continue 

do  2020  tou-1.3 

do  Z020  fov-1,3 
httou.tov)  -  0.0d  -00 
do  20Z0  tow- I. 3 

h(tou.lov)-  httt  tou,  tow)*tpm(  tov.  tow)  *  h(tou.tov) 

2020  continue 

return 
end 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 
c  C 

cccccc  this  subroutine  calculates  rho  az  and  el  for  a  given  r  and  v  ccc 
subroutine  raze 1 (r.v.rho.az.el ,to,t,rs,trm) 

double  precision  r  <0: 3 ) , v(0s 3 > . rho.az .el . to. t , rs(0»  3  > , trm<  3 , 3 ) 

doub 1 e  precision  lat . Ion , 1 st . zvec (0: 3) ,svec<0:3) ■ evec <0: 3 ) , rad . 

*  r hove(0:  3 ) . kvec(0 : 3 ) , rhovec  <0 :3) ,re<0:3) ,rse(0:3> 

Integer  Ing, f  nh, tnl, in J , Ink ,  tnl , f  nm, tnn 
character  ans 

tf  t 1 ng .eq .0)  then 

print*. 'enter  sensor  type,  land  or  space,  tn  quotes* 
read* ,ans 
tng-  10 

rad-  3. 14i53Z65359d  *00/ 1 80 . 0d-00 
kvect 1 )-  0.0d*00 
kvect 2)-  0.0 d*00 
k  vec ( 2 ) -  1.0d*00 
end  ( f 

If  < t ans . eq . ' 1 ’ ) .and . ( t nh .eq .0) )  then 

jr t nt* , * t nput  the  lat  and  Ion  of  site  tn  deg.  east*,  west-’ 
read* . lat. Ion 
lat-  lat-rad 
1  on-  Ion-rad 
tnh-  10 
end  t  f 

call  lstt*e< 1st, t, to. Ion) 
call  radstt rs. lat, 1st, t, to. ans) 


do  1 00  1  n  1  “1 ,3 

rhovelfnl)-  rlfnl)  -  ps(lnf) 

1 00  continue 

call  mag ( r hove ) 
rho-  rhoveUJ) 
do  110  1  n  J-  1,3 

zvecllnj)-  rsl inj >/rs(0> 

1 10  cont  <  nue 

call  crossl  1:  vec ,  zvec  ,evec  ) 
do  112  1nm-l ,3 

evec ( I  nm  > »  evec ( 1 nm ) /evec ( 0 ) 

112  continue 

call  crossl evec , zvec , svec ) 
do  114  1 nn*  1 , 3 

’Jvacl  Inn)*  sv«c(  Inn)/ ]vec(0) 

114  continue 

cccccc  Set  up  the  transf ormit 1 on  fop  IJK  *  tpm  SEZ 

do  120  lnl-1,3 

trmllnl.l)-  svecllnl) 
trmltnl.Zl-  evec(lnl) 
trmltnl.3)-  2vectlnl) 
cont Inue 
do  121  lnnl-1,3 

re( Innl  )«  p {  Inn  1  ) 
rsellnnll-  pa(lnnl) 
cont  <  nue 

Convert  to  SEZ  for  calculations 
do  130  Ink-  1.3 

rhovecl 1nk)«  r hovel  1 ) “trial 1 . 1 nk )  *  r hovel  2 1 “trmi 2 , 1 nk ) 

♦  phovef 3 ) *trm( 3 , 1 nk ) 

p(lnk)*  pel  1  )  “trml  1  ,  Ink  >  ♦  rel  2  )  “trial  2 , 1  nk  > 

♦  ♦  rel  3  )  “trial  3  .  I  nk  ) 

p  s  1  ink)-  rsel 1 >*trml 1 , Ink >  ♦  r  sel  2  )  “trial  2  ,  Ink) 

*  ♦  rsel 3 >“trml 3 , Ink ) 

130  continue 

If  1 rhovecl 1 ) . eq .0 .0d*00 >  then 

if  1 rhovec 1 2 ) .gt .0.0d+00)  az-  90.0d*00“rad 
if  1 rhovec 1 2 ) . It -0.0d*00>  az-  270.0d*00*rad 
If  1 rhovec 1 2 ) . eq .0.0d+00>  then 

az-  0.0d*00 

If  (rhovecl 3 > .gt.0.0d*00>  el-  90. 0d  *00“rad 
If  (  rhovecl  3  1  .  It. 0.0d*00)  el-  -90.0d*00*rad 
end  1  f 

end  I  f 

if  1  I rhovecl l ). ne.0.0d+00> .and . 1 rhovecl 2 ) .ne .0.0df00> )  then 
az-  datanl rhovec i 2)/rhovec( 1 > > 

el*-  datan  1  rhovec  i  3  > /dsqrtl  rhovec  (  1  )  “rhovec  1  1  )  +  rhovec!2)» 

♦  rhovecl 2 ) ) ) 

If  1 rhovec 1 1 ). 1 t . 0 .0d*00>  az-  az  ♦  130. 0d  *00“rad 

if  1 ( rhovec 1 1 ) .gt .0.0d*00) . and . 1 rhovecl 2 ). 1 t . 0.0d*00> )  az- 

*  az  ♦  360.0d*00“rad 
end  I  f 

return 

end 


120 

121 

ccccc 
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APPBNDIX  H 


PROGRAM  LSTSQ.F 

De  scr ipt ion 

This  program  checks  out  the  initil  runs  for  a  least 
squares  estimation  of  the  input  problem.  The  cases  used  for 
trial  runs  consisted  of  the  satellite  orbits  which  were 
numerically  integrated  around  one  orbit.  The  estimator  works 
by  mechanizing  the  summary  shown  on  the  following  page. 
Basically,  the  data  is  input,  along  with  the  truth  model  data 
that  is  run  from  program  tn.f  separately.  After  an  initial 
state  vector  is  input,  the  initial  guess,  the  least  squares 
program  takes  the  guess  along  with  the  observation  data  and 
tries  to  estimate  the  true  intital  state. 

User  inputs 

t  epo  ch 
xref ( 8) 
m  ax  i  t 


nob 
tr  op 

Variables  to  be  set 

The  Haming  Common 

Remaining  variables 

timeob(  ) 
rho  (  ) 

az  (  ) 

e  1  (  ) 

phi ( 8 , 8 ) 
h(3 ,8) 
tm  a  t  (  3 , 8  ) 
t  ob 
z  (  3 ) 

zpr  e  d ( 3 ) 

dx ( 8 . 1 ) 
ql(3 .3) 
r  e  s  i  d  (  3  ) 

work 

htql ( 8,3) 
pinv( 8.8) 
htql r ( 8 , 1 ) 
xr ef ( 8) 


time,  rho,  az  and  el  for  the  storage 
of  the  truth  model  data 

* 

H  matrix 
T  matrix 

holds  time  of  each  observation 
holds  G  matrix  values,  each  observation 
holds  G  matrix  values,  calculated  from 
xref  in  the  program 
state  vector  corrections 
Q-1 

residual  vector 

storage  for  IMSL  inverse  routine 

tTq-1t 

P" 1 

TTQ- 1 r 

state  vector  which  gets  updated 


initial  starting  time 
initial  guess  for  the  state  vector 
number  of  iterations  that  least  squares 
will  run  through  while  trying  to 
estimate  the  state 
number  of  observations  to  be  read, 
rank  of  p  matrix  (used  in  inversions) 


p  (  8 , 8 ) 

t epoch 
tmatt (8,3) 


P  matrix  (covariance) 
Initial  start  time 
T  transpose 


Subroutines  used 

mmpy 
mtr  ans 
mpr int 
eigen 


dhaming 

(See 

Appendix 

B) 

rhslb 
obser  1 

(See 

Appe  nd i x 

C) 

razel  | 

randv  . 

mag 

(  See 

Appe  nd i x 

G) 

• 

cross 

radst 

lstime 

(See 

Appe  nd i x 

A) 

Notes 

The  value  of  trop  was  important  in  calculating  the 
eigenvlaues  and  eigenvectors.  The  satellite  had  a  rank  of  6, 
whereas  the  ICBM  had  a  rank  of  8.  Difficulties  were 
encountered  with  using  trop=8,  so  it  was  left  at  6 


MMPY 


This  subroutine  multiplies  2  matrices  together,  and 
outputs  the  result.  Note  that  the  2  matricies  must  be  declared 
identically  in  and  out  of  the  routine,  and  they  must  both  be 
2  dimensional,  i.e.  (0:3, 0:4) 

MTKANS 

This  subroutine  calculates  the  transpose  of  a  matrix. 

MPKINT 

This  subroutine  prints  a  matrix. 

BIGEN 

This  subroutine  calculates  the  eigenvalues  and 
eigenvectors  for  the  filter  estimation  problems.  Note  that 
copies  are  made  to  pass  to  the  IMSL  routines  since  these 
routines  destroy  the  original  matrix. 
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9 


Non-Linear  Least  Squares  Algorithm 


1.  pick  xref (t0) »  initial  guess  for  state  vector 

-  Set  Q  and  read  in  data  for  all  observations  (G) 

-  initialize 


TTQ-1r  =  0 

.  for  each  observation  time 

-  move  5ref(tc)  to  xtt{{ti) 

-Haming  and  rhs  do  this  for  *ref'  a*so  ° 

-  calculate  predicted  data  using  *ref(t.[),  spied 
obser  does  this 

-  calculate  residual  r^=  z ^  -  zpred^ 

-  calculate  H  ,  obser  does  this 

-  calculate  T  =  H 

-  sum  TTQ-IT 

-  since  P"1  gets  reset  each  iteration,  it  is 
summed  up  inside  the  observation  loop 

-  sum  ^T-^Q-^r 

-  loop  back  until  all  the  data  is  processed 
Cal cul a  t ions 

-  P  =  [  TtQ_1T]_1 

-  5i  =  P  TTQ-1 r 

-  update  the  *ref(t0)=  xref(t0)  +  8i(t0) 

-  check  convergence  (See  page  23,  5x(i)  <  j/P  ^  ^  ) 

-  if  good,  end  with  xref(t0) 

-  if  not,  begin  at  start  with  x  f(t0)  and  reset 


T 


cccccc  print  covariance  matrix  and  find  eigenvalues 

950  format! / ,tx, 'Covar lance  Matrix  at  epoch  Is:"./, 
*  8  <  t  x . 8el 4.7,/)  ) 

print  950, p 
call  e I  gen ( pc . tr op ) 

cccccc  1 oaa  new  state  vector,  and  reset  phi 

do  960  Ibr-  1.8 

y ( Ibr , 1 ) ■  y(lbr.nxt) 
xr ef ( 1 br , 1 > ■  yilbr.nxt) 

960  continue 

call  meq 1 ( xref , 3 , 1 , xr ef u ) 

cccccc  extract  phi  matrix  In  norma  1  form 


do  985  Ibv  ■  1,8 

do  985  Ibw  ■  1,8 

985  phiflbv.lbw)  •  y ( 8* f bw+ f bv , nxt ) 

cccccc  calculate  updated  phi  matrix  at  ntiw  start  time 
call  meq 1 ( ph 1 . 8 , 8 , ph 1 c  ) 

call  11nvlf(ph1c,8.8.phlln,0. work , I er ) 

'*  *  1  1  ml.r^nxl  ohlln.fl.fl.ohftl 

call  mmpy (ph ft,8,8,p1nvn,8,p1nvol ) 
call  mmpy  < plnvol ,8, 8. phi tn,8.plnvn) 
ca 1 1  mmpy <  beta ,8,8,pinvn,8,pinvo) 
call  mtr ans ( beta . 8 , 8 . betat ) 
call  meq 1 ( p I n vo , 8 , 8 , p I nvo2  ) 
call  mmpy ( p I nvo2 , 8 , 8 .betat , 8 . p 1 nvo) 


tepoch  *  t 
idone  *  0 

pr I nt* , ' beg  I n  next  bayes  loop’ 


10000  continue 


c 


LOOP  BACK  FOR  BAYES  FILTER  LOOP 


print*. 'we  did  It,  success  with  bayes' 
end 


c 

c 


this  subroutine  makes  a  copy  of  a  matrix 
subroutine  meql <  mat7 ,mat7r ,mat7c,mat8) 


douole  precision  mat7 ( mat7r . mat7c ) 
Integer  mat7r,mat7c 

double  precision  mat8 < mat7r . mat7c  ) 

I ntager  I mf .  I  mg 

do  3000  1 mf  *  1 ,mat7r 

do  3000  lmg«  l,mat7c 

3000  mat 8 ( 1 mf , I  mg ) *  mat 7 ( I mf ,  I  mg  ) 

return 

end 


» 
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U  U 


cccccc 

cccccc 

cccccc 


cccccc 

cccccc 


do  4  20  1  be* 1 ,8 

do  420  I  bf  *  1,8 

p i nvn( tbe, Ibf ) ■  p 1 nvo! ibe, Ibf >  ♦  htqlt! tbe, Ibf ) 
cont 1 nue 

call  meq 1 ( p f nvn , 8 , 8 , p 1 nwnc ) 

have  we  Just,  flnslhed  printing  last  pass  residuals  7 

1f(  tdone  . eq .  1>  go  to  5000 

data  Is  processed ....  Improve  estimate 
invert  matrix  H  transpose  Q  Inverse  H  to  find 
covariance  P 

cal  1  1  1nvlf!p1nvnc,trop,8,p,0,work, ler) 

call  meq 1 < p , 8 , 8 , pc ) 


from  matrix 


P  *  T  transpose  Q  Inverse  r 


do  500  1  be* 1 ,8 

xmxref < Ibc , 1 > *  xref u! Ibc . 1 >-  xrefllbc.l) 
continue 

ca 1 1  mmpy ( p 1 nvo .8,8, xmxref , 1 ,pndx ) 
do  640  lbd"l ,8 

pndxphl  Ibd, i )■  pndxl  tbd,  1  )*htqlr ! Ibd , 1 ) 
cont 1 nue 

call  mmpy ( p , 8 , 8 . pndxph , l ,dx ) 
add  In  state  corrections., 
do  700  11 v  -  1.8 

xreflllv.l)  »  xreflllv.l)  ♦  dx(llv.l) 
print  iteration,  and  current  guess 

format! / ,2x ,* Iteration  * . 1 3 , / . 2x . *  state  corrections* 
,/.2x.4e20. 13,/,Zx,4eZ0. 13  1 
pr Int  720. I lc.dx 

format! /. 2x , “current  reference  trajectory  state  vector  at 
at  epoch : * . / ,2x , 4e20. 13 , / ,2x , 4eZ0. 13 , / ) 
print  740. xref 

SUCCESS  7?7777?7? 
check  convergence 

if  a  1 1  ■  0 
do  800  flu  -  1.8 

I f (  dabs ( dx ! 1 1 u , 1 ) ) . gt .0 . l*dsqrt! dabs! p! 1 1 u , 1 1 u  1 ) >  > 
if a  11  -  1 

cont 1 nue 

if!  If all  .eq.  0  >  I done  *  1 
cont Inue 

LOOP  BACK  FOR  NEXT  ITERATION  OF  LEAST  SQUARES  - 


FAILURE  for  the  least  squares  !  !  !  !  !  !  1  !  !  !  !  ! 

format! 2x . “max imum  Iteration  limit  exceeded 
without  convergence.*) 
print  900 
stop 

SUCCESS  for  the  least  squares  !!!!'.!!'.!!!!! 
cont 1 nue 

format! / . Zx , “CONVERGENCE  ACHIEVED . “ , / 

,2:<,*In  nomlnia  Gausslam  trajectorum  referent  la* ,/ , 
2x , “dec lar turn  est  estimatta* ,/ ) 
print  940 

(Inobs  ■  11  nobs  ♦  II nob 


111 


cccccc 


cccccc 

c 

cccccc 


IBB 

cccccc 


cccccc 


1  ZB 


200 


cccccc 

cccccc 

240 

250 

cccccc 


270 

260 

cccccc 


cccccc 


cccccc 

cccccc 

280 

♦ 

290 

cccccc 

cccccc 

150 

♦ 

9000 

1000 


-  OBSERVATION  PROCESSING  LOOP  - 

do  1000  ill  »  I  1  nobs. I  1  nob  +  II  nobs  -  1 

extract  each  observation 

tob  »  1 1 meob  <  1 1 1 ) 
z  (  1 )  ■  r Ho <lli) 

Z(2>-  12(111' 
z<3)-  el ( 1 1 1  ' 

NUMERICALLY  INTEGRATE  STATE  AND  PHI  TO  OBS  TIME 
the  number  of  steps  here  Is  equal  to  1  since  we 
have  dt  set  exactely  the  same  as  the  truth  data  we  read 

nstp-  1 

do  100  ilk  »  1 ,  nstp 

ca 1 1  himlngl nxt ) 

OBTAIN  MATRICES  FOR  THIS  OBSERVATION 

call  obser(tob,ql ,  zpred , h , nxt > 

MATRIX  STUFF  -  THIS  OBSERVATION 

do  120  111  ■  l.ndata 

resld(lll)  «  z(lll)  -  zpred(lll) 
lf(  ill  .It.  llnobs+5)  go  to  200 

lf((  idone  .eq.  1). and. (tit. It. II nobs +5 ) )  go  to  200 
tf((  Idone  .eq.  1 )  . and . ( 1 1 t . ge . 1 1  nobs ♦5 ) )  go  to  240 
go  to  250 
cont  t  nue 

pr tnt* , 'time,  res  ■ ' . tob , ( r es td ( 1 1m > . f 1m* 1 , ndata  ) 

If  this  ts  last  pass,  weve  already  converged, 
so  skip  matrix  calculations 

lf(  idone  .eq.  1  )  go  to  9000 
continue 

extract  phf  matrix  In  normal  form 


do  260  tin  -  1.8 

do  270  tlo  -  1.8 

phl( lln.llo)  ■  y ( 8* 1 1 o* 1 1 n , nxt ) 
cont  t  nue 

form  matrix  ******  tmat  •  h  *  phi 
call  nmpy ( h , 3 , 8 ,ph 1 , 8 , tmat > 

form  matrix  *■••**  htql  ■  T  transpose  *  0  Inverse 

call  mtrans ( tmat , 3 . 8 . tmatt ) 
call  nmpy ( tmatt . 8 , 3 .q 1 . 3 , htql ) 

form  matrix  «*•**«  htqlt  ■  T  transpose  Q  Inverse  T 


(sum  through  the  observations) 

do  290  lip  -  1,8 

do  290  1 1 q  -  1.8 

do  280  I  1 r  »  l.ndata 

htqlt( lip. 1 lq)»  htqlt( lip,  llq)+htql(  lip,  1  1  r  > 
*tmat(  I  lr .  I  lq  ) 

continue 

form  matrix  «***»»  htqlr  «  T  transpose  Q  Inverse  r 
(sum  through  the  observations) 

do  150  11s  -  1,8 

do  150  lit  *  l.ndata 

htqlr ( 1 1 s , l ) *  htqlr(lls.l) *htq l (  I  1 s , 1 1 1  >  * 
res  I d  <  tit) 


cont 1 nue 
cont 1 nue 


c 


LOOP  BACK  FOR  OBSERVATION  LOOP  OF  LEAST  SQUARES 


cccccc 


READ  IN  OBSERVATIONS 


open  < un 1 1- 1 4 . f  1 le-’tdata’ , access- ' sequent  la  1  * . status- ’ ol d ’ ) 
rewf  nd<unlt-14) 

pp 1 nt* , ’ 1 nput  the  total  number  of  observations  to  be  read’ 

read", nob 

do  30  11b  -  1 . nob 

read  ( 1 4 . " , end-30)  r ho< t 1 b ) , az < 11 b > ,e 1 { 1 1 b > . t 1 meob ( 11 b > 

30  continue 

endf 1 1e<  un 1 1- 1 4  > 
ndata-  3 

print  10. xref u , tepoch , nob .max  1 1 , 1 1  nob , 1b 1 oop . trop , 

+  beta (1.1). beta (3.2). beta (3.3), beta (4,4) .beta (5.5). 

+  beta ( 6 , S ) . beta (7.7), beta (3,3) 

cccccc  set  last  Iteration  flag 

(done  -  0 
II  nobs  -  1 

call  meq 1 ( xref u , 8 , 1 , xref ) 
do  40  1bJ-l , 8 

do  40  1b 1-1, 8 

40  p 1 nvo( 1b J ,  1b  I  )■  0.0d +00 

c -  BEGIN  BAYES  FILTER  LARGE  LOOP  - c 

do  10000  Ibg-  1,1b loop 

c -  BEGIN  ITERATION  LOOP  -  NONLINEAR  LEAST  SQUARES  - c 

dt  »  t1meob(2>  -  ttmeob(l) 
do  9999  lie  -  1. max  It 

cccccc  REINITIALIZE  NUMERICAL  INTEGRATION  PARAMETERS 

t  -  tepoch 
mode  »  1 
n  -  72 

cccccc  les  are  new  reference  traj  guess 

do  50  lid  -  1,8 

y(  1 1d  ,  1 )  -  xref ( 1 1d . 1 ) 

50  continue 

cccccc  phf  initial  conditions 

do  60  lie  »  9,72 

60  y( 1 le. 1 )  -  0.0d+00 

do  70  Ilf  -  9,72.9 
70  y(llf.l)  -  1.0d+00 

cccccc  initialize  haming  and  reset  the  time 

nxt  -  0 

cal  1  hamlngl nxt) 
t-  tepoch 

cccccc  INITIALIZE  BUFFERS  FOR  MATRIX  PRODUCT  ACCUMULATION 

do  80  1 lg  •  1,8 

htqlr f I lg,  1  )  -  0.0d+00 
do  80  1 lh  -  1 ,8 

80  htqltt I lg, I 1h>  -  0.00d*00 

cccccc  print  first  or  last  pass  re didual  headers  when  necessary 

90  format ( 2x . *F irst  Pass  Residuals:  ",/> 

95  format! 2x . "Last  Pass  Residuals:*./) 

if ( lie  .eq.  1)  print  90 
ifMdone  .eq.  I  1  print  95 


» 
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u  o  u  o  u 


ccccececcccccceccceccccececcececeeccccccccccecccececcccccecccccccceccece 
program  bayes 


c  nonl Inair  leastsquares  algorithm  c 

c  c 

c  This  program  accomplishes  a  nonlinear  least  squares  algorithm  c 

c  for  the  problem  of  estimation  of  launch  vehicle  performance  c 

c  parameters.  The  program  uses  obser  to  calculate  the  Q  Inverse  c 

c  the  appropriate  H  matrix,  and  the  observation  matrix.  The  c 

c  program  also  uses  dhamlng  to  numerically  Integrate  the  state,  c 

and  rhs  to  calculate  the  EOM  and  EOV.  c 

c 

Capt.  Oave  Vallado  1934  c 

c 


ceccccccccccccccccccccccccccccccccccccccccccccccceccccecccccccccecccccc 
cccccc  The  common  terms 

common  /ham/  t , y< 72 , 4 ) ,f ( 72, 4 ) , err < 72 ) , n ,dt , mode 
double  precision  t , y . f ,er r , dt 
Integer  n.mode.nxt 

ccccc  The  other  terms 

double  precision  t1meob(500)  ,rho(500)  ,az(S00)  ,el  (500) , 

ph1(8.8),h(3.8> , tmatl 3,8) ,z<3  ) ,zpred(3> ,dx<8. 1  >  , 
ql(3.3),res1d(3), tob .work ( 3 ) . htq 1(8,3) ,p(nvo(8,8)  , 
htqlr ( 8,l),xref(8,l).p(8,8), tepoch . tmatt ( 8 , 3  )  , 
p 1 nvn<  8 , 8 ) , xref u<  8,1), xmxref ( 8,1) ,pndx( 8,1), 
pndxph (8,1), htq lt( 8,8),ptnvnc(8.8).pc(8.8),p1nvol 
(8,8) .phi c( 8.8) .phi ln( 8,8) ,phtt( 8,8) .beta (8,8) , 
betat ( 8 . 8 ) ,p  1  nvoZ( 8 , 8 ) 

1  nteger  11a, 11b, 11c, ild, lie, 1 1  f  .max  It ,  nob  ,  trop  ,  1 1  nob  .  1 1  n'-us 

cccccc  REAO  IN  INT1AL  DATA  AND  ALL  CONTROL  PARAMATERS 

print*, *  Input  epoch  time’ 
read" .tepoch 

pr 1 nt* , ’ 1 nput  Initial  state  vector  guess,  xref’ 

read* ,xrefu( l . 1 ) , xref u ( Z . 1 ) ,xrefu(3, 1 ) .xreful 4, 1 ) ,xrefu( 5, 1 > . 

♦  xrefu(6.1).xrefu(7,l),xrefu(8,l) 

pr Int* ,’ Input  the  max  Iterations’ 
read* .max  It 

pr 1 nt* , ’ 1 nput  the  rank  of  p’ 
read* , trop 

pr 1 nt* , ’ 1 nput  the  number  of  bayes  loop  Iterations’ 
read* , 1b  loop 

pr 1 nt* ,* 1 nput  the  number  of  data  points  to  be  read  each 

♦  least  squares  run’ 
read* , I  1  nob 

pr 1 nt* ,’ 1 nput  beta,  for  the  fading  memory  ’ 

read* .beta ( 1.1) , betat 2.2) ,beta( 3 .3 ) , beta! 4.4) ,beta( 5 .5  )  , 

♦  beta ( 6 . 6 ) ,beta< 7 , 7 ) .beta ( 3 , 8 ) 

cccccc  print  out  Input 

10  f or mat < / . 20x . " NONLINEAR  BAYES  FILTER* ./ ,2x  . 

♦  "initial  state  vector  : ’ , / ,Zx . 4eZ0. 13 , / . Zx , 4eZ0. 13 , / . 

♦  "Initial  time  :  ",f8.6,"  #  of  data  points  *,14./,2x, 

♦  "max  LS  iterations  :  ",I8,"  *  of  bayes  chunks  :  ",i4,/,2x, 

♦  "max  bayes  Iterations  :  ",14,“  rank  of  P  ",lll./,2x, 

♦  "Beta  matrix  *  *,8f6.3,) 


Bayes  Filter  Algorithm 


1.  Pick  xrefu(tepoch) »  initial  guess  for  state  vector 

Set  Q  and  xtflf  =  xrefu 
read  all  data  and  store  in  z 

set  P_ 1 ( - )  =  0  tepoch  =  0 

2.  For  each  Bayes  iteration 

3.  For  each  least  squares  iteration 

Set  t  =  [13  :  tTq"1!  -  0  :  TTQ_1r  -  0 

4.  For  each  observation  tine 

“ov®  iref(tepoch)  to  iref(ti) 

haming  and  rhs  do  this  to  *£ef  also 

calculate  predicted  data  using  xref (t^  ~  zpred 
obser  does  this 

calculate  residuals  f j  =  z  ^  -  zpred^ 
calculate  H  -  obser  does  this 
T  -  H  A 

sub  £tT«"1t 
sum  53  TTQ-1  r 

.  loop  back  until  each  data  segment  is  processed 
P-l(+)  =  p-l(-)  +  Tt«_1T 

6x  =  P(+)  (P-I(-) (x(-)-xref )  +  TTQ_1r  ) 

Xref^o*  ■  ^ref<to)  "  1 

determine  convergence  (See  page  23) 
no  y  *  * 

loop  back  for  another  L.  S.  P“l(~)  =  P-M+) 

iteration  *ref^*i^  =  y(*»nxt) 

*  'refu^i^  =  *r  ef  ^  *  i 

P-1 (-)=  -lTp-1^-1 

tepoch  =  * 

P-I(-)  =  p  P_1<-)  p 


p i nv  n ( 8 , 8 )  P_1(+) 

pinvnc(8,8)  copy  of  pinvn 

pinvol(8,8)  update  of  pinvo  after  each  least  squares 
run 

h  t  ql  r  (  8 , 1 )  TTQ_1r 

xref(8,l)  state  vector  which  gets  updated 

zrefu(8>l)  initial  state  for  each  bayes  loop 

p(8,8)  P  matrix  (covariance) 

pc(8>8)  copy  of  p  matrix 

tepoch  Initial  start  time 

tmatt(8,3)  TT 

xaxref(8,l)  xrefu  -  xref 

Counters  and  Disc  holders 

ilb,  il  j  ,  ibi ,  ibg ,  ibe  ,  ibf  ,  ib  c  ,  ibr ,  ibv,  ibw  ,  il  c ,  il  d,  ile, 
ilf.  il  g »  ilh,  ili,  ilk,  ilm,  iln,  ilo,  ilp,  ilq,  ilr,  ils,  ilt, 
ilv, il u, pndx (8,1) ,pndxph(8,l) 

Subroutines  used 

meql 
mmpy 
mtr  ans 
mpr int 
eigen 
dham ing 
rhslb 
obser 
raz  el 
randv 
mag 
cross 
radst 
1 s  t ime 

MEQL 

This  subroutine  makes  a  copy  of  a  matrix.  This  was  used 
in  the  Bayes  filter  program. 


(See  Appendix  H) 

(See  Appendix  B) 
(See  Appendix  C) 

( See  Appendix  6) 
(See  Appendix  A) 


APPENDIX  I 


PROGRAM  BATES. F 


Description 

This  program  mechanizes  the  Bayes  filter  discussion  in 
chapter  3  and  incorporates  the  summary  that  is  shown  on  the 
following  page.  It  reads  the  truth  model  data,  proceeds  to 
input  user  data,  and  then  it  calculates  the  state  vector  as 
each  segment  of  data  is  processed.  The  program  is  very 
similar  in  operation  to  the  lstsq.f  program,  with  the 
major  differences  discussed  in  chapter  3. 


User  Inputs 

tepoch 
xrefu( 8) 
maxi  t 
trop 
ibl oop 
i  1  no  b 

beta 


initial  start  time 

initial  state  vector  guess 

max  number  of  least  sqaures  iterations 

rank  of  P  matrix 

max  number  of  bayes  loop  iterations 
number  of  data  points  read,  each  least 
squares  iteration 

Weigthing  matrix  for  covariance  matrix 


Variables  to  be  set 


The  haming  common 
Remainging  variables 


timeob(  ) 
rho  (  ) 

az  (  ) 
e  1  (  ) 

ph i ( 8 . 8 ) 
phi in( 8 , 8 ) 
phit (8.8) 
phic (8,8) 
h( 3 , 8 ) 
tmat (3,8) 
tob 
z  ( 3 ) 

zpr  ed ( 3 ) 

dx  (  8 . 1 ) 
ql(3 .3) 
re  sid( 3 ) 
work 

htql ( 8,3) 
pinvo ( 8 . 8 ) 


time,  rho,  az  and  el  for  the  storage 
of  the  truth  model  data 


copy  of  4  matrix 
H  matrix 
T  matrix 

holds  time  of  each  observation 
holds  G  matrix  values,  each  observation 
holds  G  matrix  values,  calculated  from 
xref  in  the  program 
state  vector  corrections 
Q~ 1 

residual  vector 

storage  for  IMSL  inverse  routine 
jTq-1>j> 

P-1(-) 


I 


do  2020  I la*-  1 , 3 

stigpC  llu,  1  >•  real <wp< 1 1ae> > 

3020  csntlnua 

pr f nt* , ' not*  that  th«s*  values  are  all  In  random  ord*r’ 
pr I nt* ,’ e 1 genva 1 ues  equal  for  the  covariance  matrix’ 
call  matprt ( st 1g . 8 , 1 > 

print*. 'eigenvectors  equal  for  the  covariance  matrix' 
call  matprtl st Igv , 8 . 8 ) 

cccccc  now  calculate  error  alltpsotd  axis  lengths,  do  conversions 
do  2040  11  ad- 1.8 

st!g*r( Had, 1 )-  dsqrt(st1g< 1  lad, 1 ) ) 

2040  continue 

do  2060  tlaf-1.3 

st Iper ( 11 af , 1 >«  < dsqrt ( st Igp ( 1 laf , 1 ) > ) *6378 . 1 45d+00 
st Iger ( 1 1 af , 1 ) -  st 1 ger < II af,  1  1*6373. 1 45d*00 
2060  continue 

do  2080  1  lag-4,6 

st Iger < 1 1 ag , 1 )-  st Iger < 1 1 ag , 1  1 *7 . 90563828d+00 
2080  continue 

print*, 'the  axis  lengths  for  the  covariance  matrix  are' 
call  matprt ( st Iger , 8 . 1 ) 

print*, 'the  axis  lengths  for  the  position  components  are’ 

call  matprt< sttper , 3 , 1 1 

return 

end 


1 ncl ude  * /en/en84d/dva 1 lado/obser ' 
Include  ’ /en/enS4d/dva 1 1 ado/dham I ng ' 
Include  ' /*n/en84d/dva 1 lado/rhslb' 


» 


» 
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ccccccccccccccccccccccccccccccccccccecccccecceccccccccccccccccccccccccce 
c  c 

c  this  subroutine  forma  the  transpose  of  a  matrix  c 

subroutine  mtrans < mat 4 , mat4r , mat 4c , mat 5  ) 

douale  precision  mat4 < mat4r , mat4c ) , mats ( ma t4c , mat4r ) 

Integer  mat4r,mat4c 

Integer  Imd.ime 

do  4020  Imd-I,mat4r 

do  4020  1 me-  1, mat 4c 

mats <  1  me , 1 md  )  -  mat 41 Imd , Ime) 

4020  continue 

return 
end 


( 


f 


l 


cccccccccecccccccccccccccccccccccccccccccccccccccccccccccccecccccccccccc 
c 

c  this  subroutine  prints  a  matrix 

subroutine  matprtl mats , mat6r ,matSc ) 

double  precision  matSImatSr ,mat6c) 

Integer  mat6r,matSc 

I nteger  Imh ,1ml 

4040  for  vat ( 101  lx. el  2. 6) ) 

do  4030  1mh«l,mat6r 

writs!*, 4040)  (mat6( 1mh,im1),1m1-l . mat 6c ) 

4030  continue 

return 
end 

cccccecccccecccccccccccceccccccccccccececccceccccccceecccccceccccecccccc 
c  c 

c  this  subroutine  calculates  the  eigenvalues  and  eigenvectors  c 

subroutine  e 1 gen( p , trop ) 

double  precision  p( 8,8) 

Integer  trop 

douale  precision  works ( 1 G ) ,pon 1 y< 3 , 3 ) , st  1g< 8 , 1  )  , st tgv( 8 . S > , 

♦  stiger(8.1),st1per(3,l),stlgp(3,l), workjp 1 6 ) 

douale  complex  w( 8 ) , ze 1 g < 8 , 8 ) , wp ( 3 > , ze 1 gp ( 3 . 3 ) 

1 ntager  1 1  a  .  1 1 aa ,  1 1 ab , 1 1 ag , 1 1 af ,  1 1  ad .  1 1 ae .  1 1 ac 

cccccc  calculate  eigenvalues  and  eigenvectors 
cccccc  get  position  only  components 

do  1080  1  lab- 1.3 

do  1080  i lac- 1,3 

1080  ponlyl 1  lab, 1  lac)-  p(llab.llac) 

call  e I grf ( p , trop , trop , 1 , w, ze ig , 16 .works , t er ) 

ca l 1  e Igrf (pon ly,3,3,l,wp,zelgp,6 , worksp , 1 er  > 

cccccc  transform  from  complex  values  to  real  values 

do  2000  I  la-2. 8 

stlgl 1  la . 1 )-  reallwlllal) 
do  2000  i 1 aa-  l  .  8 

st Igv ( 1 1 aa , 1 1  a ) •  rea  1  <  ze  tg(  1  laa  ,  1  la  )  > 

2000  continue 


k 
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n  n 


32  0 


cccccc 

cccccc 


700 


c - — 

9999 

cccccc 

300 


f ormat! /, Zx current  reference  trajectory  state  vector  at 
at  epochs  * . / ,2x,4e20. 13  ,/,2x  , 4e20. 13 ,/ ) 
print  82 0.xref 

SUCCESS  7???????? 
check  convergence 

If a  1 1  *  0 
do  700  11 u  -  1.8 

If (  dabs(dx( 1 1 u , 1 ) ) . gt .0. l*dsqrt(dabs(p( 1 1 u , I  1 u ) ) ) ) 

I  f  a  1  1  -  1 

cont 1 nue 

Ifflfall  .  eq.  0  >  (done  ■  1 

-  LOOP  BACK  FOR  NEXT  ITERATION  - 

continue 

failure  minim ! 


format! Zx , "max Imum  Iteration  limit  exceeded 
«■  without  convergence. " ) 

print  900 
stop 

cccccc  SUCCESS  It!  1 1 1  111  111  l 
5000  continue 
850 

♦ 

♦ 


format {/.2x, "CONVERGENCE  ACHIEVEO.  *  ./ 

. ,2x."In  nomlnla  Gaussian  trajectorum  referent  la *,/ , 
2x , " dec  1 ar  <  urn  est  estlmatla" ,/ ) 

print  820 


cccccc  print  covariance  matrix 

950  format! /. 2x , "Covar lance  Matrix  at  epoch  las",/, 

+  8 ( Ix,8el4.7./)  ) 
print  950, p 

call  e1gen(p ,trop ) 

end 

ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc cccccc 
c  c 

c  this  program  multiplies  2  matrlcles  together  c 

subrput 1 ne  mmpy ( matl . mat  1 r , matl c , mat2 , mat 2c , mat3 ) 

double  precision  mat 1 ( mat 1 r , mat  1 c ) , mat2 ( mat  1 c , mat2e > , 

'  *  mat3!matlr ,mat2c ) 

Integer  Ima, 1  mb , I  me , mat  1 r , mat 1 c , mat2c 

do  4000  Ima  «  l.matlr 

do  4000  Imb  *  l,mat2c 

mat3( Ima, (mb)  •  0.00d +00 
do  4000  Imc  *  l.matlc 

mat3( Ima , Imb >  »  mat3 ( Ima . Imb >  » 

*  matl ( Ima , Imc )  *  mat2 ( Imc , I  mb ) 

4000  continue 

return 
end 


*  I 


r  l  rj 


4 


cccccc 

cccccc 


i  f  {  f "!  I  .It.  10)  go  to  200 

if((  ido na  .eq.  1 ) . and . i i li . 1 t . 10 ) )  go  to  200 
lf((  ldone  .eq.  1  >  . and . i  i 11 . ge . 1 0 ) )  go  to  240 
go  to  250 

continue  ....... 

or ( nt" , ’ t lme .  res  »’ , tob . ( res ld< I 1m> , llm"l • ndata ) 

If  this  is  last  pass,  weve  already  converged, 
so  skip  matrix  calculations 

if(  ldone  .eq.  1  )  go  to  9000 
cont 1 nue 

extract  phi  matrix  In  normal  form 

do  105  I  In  -  1,8 

do  106  i lo  -  1.8 

phillln.Ilo)  -  y(8*11o+l In.nxt) 
cont  Inue 


form  matrix 


cccccc 

cccccc 


cccccc 

cccccc 


call  mmpy ( h , 3 , 8 ,ph I , 8 , tmat ) 

form  matrix  **«*»*  htql  «  T  transpose  *  Q  Inverse 

call  mtr ans( tmat , 3 , 8 , tmatt > 
call  mmpyl tmatt , 8 , 3 ,q 1 , 3 , htql > 

form  matrix  *»■**"  plnv  -  T  transpose  Q  Inverse  T 
(sum  through  the  observations) 

do  140  Up  -  1.8 

do  140  1 1 q  ■  1,8 

do  130  llr  «  1 , ndata 

p  I  n v (  lip,  I  1  q  >  ■  plnvlllp,  11 q  >  *htq 1 (I lp, I lr  > 
"tmat( llr, I Iq ) 

continue 

form  matrix  •»****  htqlr  «  T  transpose  Q  inverse  r 
(sum  through  the  observations) 

do  150  1 1 s  •  1.8 

do  150  lit  ■  1, ndata 

htqlr (  Ils  ,  1  )-  htqlr (  i 1 s , 1 >*htql (  I  Is .  1  It  >* 
resld(Ht) 


LOOP  BACK  FOR  OBSERVATION  LOOP 


cccccc 

cccccc 

cccccc 


cont 1 nue 
cont 1 nue 

have  we  Just  flnsihed  printing  last  pass  residuals  ? 

tf(  ldone  .eq.  1)  go  to  5000 

data  is  processed  ....  Improve  estimate 
Invert  matrix  H  transpose  0  inverse  H  to  find 
covar lance  P 

call  1  Invl f (p inv . trop , 8 ,p ,0,work , ler ) 

from  matrix  ******  dx  *  P  *  T  transpose  Q  Inverse  r 
call  mmpy < p , 8 , 8 , htql r , 1 , dx > 
add  In  state  cor rect Ions . . 
do  800  Hv  -  1.8 

<ref(ilv)  *  xref(llv)  *  dxlllv.l) 
orlnt  Iteration,  and  current  guess 

format!/. 2x. “iteration  ' . 1 3 . / , 2x . ’ state  corrections” 
,/ ,2x,4e20. 13,/ ,2x , 4e20 . 13  ) 
or  1 nt  720.  I  1c , dx 
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r 
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-  3EGIN  ITERATION  LOOP  -  NONLINEAR  LEAST  SQUARES  -  - c 

dt  »  t(meob<2)  -  tfmeob(l) 
do  9999  tic  *  I.maxlt 


cccccc 


cccccc 


50 

cccccc 

51 

52 

cccccc 


cccccc 


60 

cccccc 

63 

64 


c 


cccccc 


cccccc 

c 

cccccc 


30 

cccccc 


cccccc 

100 

cccccc 


REINITIALIZE  NUMERICAL  INTEGRATION  PARAMETERS 

t  *  tepoc*- 
node  «  1 
n  »  72 

tcs  are  new  reference  traj  guess 

do  S0  lid  *  1 , a 

y( 1 1d. 1  )  *  xref <  ( Id) 
cont  t nue 

phi  initial  conditions 

do  SI  lie  -  9,72 

ydle.l)  -  0 . 0d*00 
do  52  Ilf  -  9,72,9 

yd  If.  1)  -  1.0d*00 

Initialize  hamlng  and  reset  the  time 

nxt  *  0 

cal  1  hamlng(nxt) 
t»  tepoch 

INITIALIZE  BUFFERS  FOR  MATRIX  PRODUCT  ACCUMULATION 

do  60  tig  ■  1.3 

htq 1 r  *  1  1 g , 1 )  -  0.0d«-00 
do  60  1 1 h  “  1 , B 

p 1 nv(  I  lg  ,  1 1 h  )  *  0.00d *00 

print  first  or  last  pass  redldual  headers  when  necessary 

forsatt 2x . “F 1 rst  Pass  Resldualsi  ",/> 
format ( 2x . "Last  Pass  Resldualsi*,/) 

Iflllc  .eq.  1)  print  63 
If ( I  done  .eq.  1  I  print  64 

-  OBSERVATION  PROCESSING  LOOP  - c 

do  U00  111  »  1  ,  nob 

extract  each  observation 

tob  •  t 1 meob (111) 
i ( 1 ) *  rhot lit) 
i  (  2 '•  a*<  1  1  1  ) 
i  (  3  >»  ellllli 

NUMERICALLY  INTEGRATE  STATE  AND  PHI  TO  OBS  TIME 
che  number  of  steps  here  Is  equal  to  1  since  we 
iave  at  set  exactely  the  same  as  the  truth  data  we  read 

nstp*  l 

lo  80  Ilk  «  I ,nstp 
ca 1 1  hamt  ng<  nxt ) 

OBTAIN  MATRICES  FOR  THIS  OBSERVATION 

:a11  obser ( tob , ql , zpred , h , nxt > 

4ATRIX  STUFF  -  THIS  OBSERVATION 

do  100  111  •  l.ndata 

resld(lll)  *  z (  ill)  -  zpred(fll) 

selectively  print  out  the  Iteration  data 
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cccccccccccecccccccccccccccccccccccccccccccccecccecccccccecceccccecccccc 


program  l3tsq 


nonlinear  least squares  algorithm 

This  program  accomplishes  a  nonlinear  least  squares  algorithm 
for  the  problem  of  estimation  of  launch  vehicle  performance 
parameter?.  The  program  uses  obser  to  calculate  the  Q  Inverse 
the  aoproprlate  H  matrix,  and  the  observation  matrix.  The 
program  also  uses  dhaming  to  numerically  Integrate  the  state, 
and  rhs  to  calculate  the  EQM  and  EOV. 


Capt.  Dave  Vail  ado 


1984 


c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 


cccecccccccccccccccccccccccccccccecccccccceccccceccccccccccccccccccccccc 


cccccc  The  common  terms 

common  / ham/  t , y ( 72, 4 ) , f ( 72 , 4 ) , er r ( 72 > , n , dt .mode . 
double  precision  t,y,f,err,dt 
integer  n,mode,nxt 

ccccc  The  other  terms 

double  precision  t imeob < 500) , rho( 500) , az ( 500 ) . el ( 500 ) , 

♦  ph 1 ( 8 . 8 ) , h ( 3 , 8 ) , tmat <  3,3) .  z<  3) , zpredl 3 > ,dx(8, 1 > . 

+  q 1 ( 3 , 2  > , res  Id <  3  )  , tob . work ( 8  > , htq 1(8, 3), pi nv (8, 8), 

♦  htq 1 r ( 8, 1 ) . xref <  8 ) , p <  8 , 3 ) , tepoch , tmatt ( 3,3) 
integer  11a, Mb, lie, lid, lie, I  If .max  It. nob.tr op 

cccccc  READ  IN  INITIAL  GUESSES  FOR  STATE  VECTOR.  CONTROL  PARAM 

pr 1 nt* , ’ 1 nput  epoch  time' 
read* .tepoch 

print*, ’ Input  Initial  state  vector  guess,  xref ’ 
read* , xref ( 1 > . xref ( Z I , xref <  3 1 . xref { 4 ) , xref ( 5 ) , 

♦  xref (6) .xref (7) .xref (8) 

pr 1 nt* ,’ input  the  max  Iterations’ 
read* , max  it 

pr Int", ’ Input  the  rank  of  p* 
read*,trop 

cccccc  print  out  Input 

S  format!/. 2x. “NONLINEAR  LEAST  SQUARES’ ,/, 2x . 

+•  "epoch  time  :  ’  ,  e20.  1  3  ,  /  ,  2x  , 

+  -Initial  state  vector : “ , / . 2x , 4e20. 1 3 , / , 2x . 4e20 . 1 3 . / 

♦  ,  2x, "maximum  1  ter at  tons : * ,  1 3, *  rank  of  p**,ll/> 
print  6 , tepoch , xref .max  1 1 , trop 

cccccc  READ  IN  OBSERVATIONS 


open(un1t"14,flle"’tdata' .access" 'sequential ' ,status«’o1d’ ) 
revl nd< un 1 t"l d  ) 

pr Int*, ’ input  the  number  of  observations  to  be  read’ 

read* , nob 

do  30  11b  ■  1 . nob 

read  ( 1 4 , * , end"30 )  r ho( 1 1 b ) , az ( 1 1 b > , el ( 1 1 b ) . 1 1 meob ( 1 1 b ) 
30  continue 

endf  tle(unlt"14) 
ndata"  3 

cccccc  set  last  Iteration  flag 
idone  *  0 


I 


I 


P 


I 


r.v. 


,*^  / 


99 


APPENDIX  J 


I CBM  Test  Kessits  100  Data  Point  Case 

initial  state  vector 

x  y  z  xdot 

-.13261130912900*00  5769695352 1 200+00  . S059282248600e-00  - . I025944089500e-02 

■  ydot  zdot  Ve  M 

4449126533300e-02  . 627633369 1 300e-02  . 3 1 7298255 1 730e-00  . 4479S375S8632e+01 

initial  time  :  .002000  sec  #  of  data  points  400 

max  LS  Iterations  8  *  of  each  bayes  chunk  :  100 

max  bayes  Iterations  :  3  rank  of  P  3 

Beta  matrix  =»  1.000  1.000  1.000  1.000  1.000  1.000  .500  .500 

T’tan-IIIB  launched  from  43  N,  132  £  1  OU  elevation 
Radar  site  at  52.6  N,  174.1  E,  1  DU  elevation 

First  Pass  Residuals: 

time,  res  »  . 249577852e-02  . 209 1 86890e-08  - . 204638340e-06  . 275503849e-05 

time,  res  =  . 299 1 55705e-02  . 6 1 1 4 1 07 87e-09  - . 207843734e-06  . 275 1 1 2060e-06 

time,  res  =  . 348733557e-02  - . 1 2001 3763e-08  - . 209707052e-06  . 2748055 1 5e-06 

time,  res  -  . 3983 1 1 409e-02  - . 3429 1 2500e-08  -. 20956 1 260e-06  . 274554077e-06 

time,  res  =  . 447889262e-02  - . 6 1 62901 48e-08  - . 2 1 6663032e-06  . 274406923e-06 

Iteration  1 
state  corrections 

.7524464312042e-03  -  .  1 4688 1  1 488669e-08  .  569229 1 303397e-07  .  3335708465224e-05 

. 2730453261049e-07  3 1 86  85790656 1 e-07  . 2 1 769 48652099e-04  -.  23435 10440430e-03 

current  reference  trajectory  state  vector  at  etlmo  1: 

-. 13261 1331604Se+00  - . 5769695366803e+00  .  30592823 1 7829e*00  -. 102260838 1035e-02 

-.44491 09 233767e-02  . 6276301 822 72 1 e-02  . 31 7320024G595e*00  . 4479352707588e+01 

time,  res  -  . 249S77852e-02  . 75 10604 10e-08  . 1 10439458e-08  -.5716073900-09 

time,  res  =>  . 299 1 55705e-02  . 788293635e-03  . 392 1 37394e- 10  - . 46 1 485322e-09 

time,  res  »  .3487335570-02  .8091028720-08  .4573337260-09  -.3587296000-09 

time,  res  =  . 3983 1 1 409e-02  . 80S920647e-08  . 30880 1 629e-08  - . 298545372e-09 

time,  res  =  . 447889262e-02  . 77 1 1 1 8S28e-08  - . 1 3 1 27 1 349e-08  - . 237021385e-09 

iteration  2 
stato  corrections 

-.56655966353950-08  .10990849317900-08  . 298873939 l 7 1 3e-09  .10672808531760-07 

-.279261824431 le-07  . 34055 76 1 84837e-07  - . 2288203826774e-04  . 29962 1 94 1 9247e-03 

current  reference  trajectory  state  vector  at  time  1: 

-. 13261 13072701e*00  -. 57696953558 1 7e+00  . 30592823208 1 8e-00  - . 1022597708226e-02 

- . 4449137159950e-02  . 6276335878483o-02  . 3 1 7297 1 4262 1 3e-00  . 4479652329530e+01 

time,  res  =  . 249577852e-02  . 1 73993853e-08  .1111872590-08  . 52 1 426 1 3 1 e- 10 

time,  res  =  . 299 1 55705e-02  . 2 1 201 4 89 1 e-08  . 473456829e- 10  . 47943 1 58 1 e- 10 

time,  res  =  . 348733557e-02  . 2337 10567e-08  . 466052752e-09  . 450903946e- 10 

time,  res  ■  . 3983 1 1 409e-02  . 23 1 5 1 84 87e-08  . 309726567e-0O  . 804860865e- 1 1 

time,  res  «  . 447839262e-02  . 1 978 1 5 4 92e-03  -.1303000150-08  - . 1 9608 1 64 le- 10 

Iteration  3 
state  corrections 

.19184533323560-12  - . 3375004459828e- 1 3  -. 1 479374 1 609 1 4e- 1 3  - . 55 1 3572843075e- 1 2 
-.2165436384948e-12  .  4637224828259e-  1  2  . 107906708 1 32 le-08  . 6 1 2269999544 1 e-08 

current  reference  trajectory  state  vector  at  tine  l: 

-. 13261 13072699e*00  -. 57696953558 1 8e+00  . 80592028208 1 8e-00  - . 1022597708778e-02 

- . 4449137160166e-02  . 6276835 8 789 4 7e-02  . 3 1 7297 1 437003e-00  . 4479652335653e+01 

Last  Pass  Residuals: 

time,  res  ■  . 249577852e-02 

time,  res  *  . 299 1 55705e-02 

time,  res  *  . 348733557e-02 

time,  res  =*  .  3983  l  1  409e-02 

time,  res  «  . 44 7839262e-02 

CONVERGENCE  ACHIEVEO. 

In  nominla  Gaussian  trajectorum  referentfa 
declarium  est  estimatla 


.  174013353e-08 
. 2 1 2034339e-08 
. 233729948e-08 
.2315377330-08 
. 197834636e-08 


.111  1 37204e-08 
■473452338e-10 
. 46605241 9e-09 
. 309726567e-08 
-. 1 30299932e-0O 


.521 45501 5e- 10 
. 479293376e- 10 
. 45050241 3e-10 
.  7970971 1 le-1 1 
-.  1  97327623e- 10 
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NEXT  BAYES  LOOP 
tepoch  »  time  2 


F-rst  Pass  Residuals: 


time,  res 
time,  res 
time,  res 
1 1  me ,  res 
1 1  me ,  res 


.  5207363090-01 
.5256940940-01 
. 530651C79e-01 
. 5356096650-01 
.3405674500-01 


-.211587923e-08 

1966340430-08 

-.220760424e-08 

-,303361597e-08 

-.264041814e-08 


. 254245 1 36e-08 
. 237330666e-08 
. 1734363 10e-0G 
. 398389 1 38e-08 
.  2502'J5296e-08 


-.539672231e-10 
-. 1 1 549408 le-09 
- . 5 1355309 1 e- 10 
-.496343557e-10 
-. 13861 S302e-09 


Iteration  1 
state  corrections 
.4772394463467e-05 
.30647923951590-04 


-.10891071151690-05 
-. 10291 13298157e-03 


.30380213247660-07 
.  1 736224 106006e-01 


-.73218669907980-04 

-.16761369585360+00 


current  reference  trajectory  state  vector  atat  time  2: 

-.13273933612930+00  -  .  5775486202332e  +  00  .  8068 1 65467056e-00  -.46506390505950-02 

- . 1983379393742e-01  . 3285 1 47722390e-0 1  . 335 1 593347604e-00  . 43 1 2038639799e+01 


time,  res 
time,  res 
time,  res 
time,  res 
time,  res 


.5207363090-01 
. 525694094e-0 1 
.5306518790-01 
.5356096650-01 
.5405674500-01 


.4831753900-05 
. 4791 1 4335e-05 
. 474885516e-05 
. 4704683 43e-05 
.466042017e-05 


. 1948921650-06 
. 1636520540-06 
. 1329802600-06 
. 1062506400-06 
.7681055820-07 


-.2383523290-05 
-. 20305 1882e-05 
-. 1 69085454e-05 
-. 1 36435899e-05 
-. 105069666o-05 


iteration  2 
state  corrections 
. 7615219313611e-06 
.25566618284960-05 


-. 1334827757042e-06 
.91453310317740-06 


- . 586 1 886219 1 36e-07 
.94940455575280-03 


-.14006205336200-04 
- . 2037972626567e-02 


current  reference  trajectory  state  vector  a  t  time  2: 

- . 1327386246073e+00  -. 57754C7537 1 600+00  . 3068 1 64330868e+00  - . 4664645255932e-02 

-.19331242275590-01  . 3285239 1 75701e-0 l  . 336 1087393 1 6 le-00  . 43 10000667 1 72e+01 


time,  res 
time,  res 
time,  res 
time,  res 
time,  res 


. 52073G309e-0L 
.5256940940-01 
.5306518790-01 
. 525609665e-01 
.5405674500-01 


.5599899790-05 
. 555 1 82063e-05 
.5501795920-05 
. 54496 1 655e-05 
.53970712/0-05 


. 1 9533224 le-06 
.1647753050-06 
. 1352787020-06 
. 1 101972350-06 
.8290906520-07 


- . 237775035e-05 
- . 203369292e-05 
-.1708921410-05 
-. 140327332e-05 
- . 1 1164221 le-05 


Iteration  3 
state  corrections 

. 3 1 50937 1 76092 e-08  - . 1 4 L0478678937e-08  -. 6G6909635376 le-09  -.13790148889820-06 
.13710027706860-07  . 2004S30430905e-07  -. 24 1 38504 773 1 6e-05  .29073436479990-04 


current  reference  trajectory  state  vector  at  e  tine  2: 

-.12273361645640+00  - . 57754C755 1 264e+00  . 3068 1 6437 4 1 99e-00  -.46647031574200-02 

-.19031223565560-01  . 328524 1 1 8023 1 e-01  . 336 1063704657e-00  . 43 10029740669e+01 


time,  res 
time,  res 
time,  res 
time,  res 
time,  res 


.5207363090-01 
. 525634094a-01 
,530651879e-0l 
. 535609665e-0L 
.5405674500-01 


.5608128160-05 
.555997694e-05 
. 55098784 le-05 
.545762345e-05 
. 5  40500077 e- 05 


. 195234734e-06 
. 16473281 3e-06 
. t  3524053 le-06 
.  1  10l64134.»-06 
. 328314448e-07 


- .237744213e-05 
- .203342796e-05 
17037028Ge-05 
-.  1  403  10431)0-05 
-.11 1630609e-05 


Iteration  4 
state  corrections 
- . 28343825457900-09 
- . 90058443250260-09 


. 49664730912650-10 
-. 45372950386 19e-09 


.21851917734700-10 
. 4248777543234e-07 


. 53068448937740-08 
-.44652675799660-06 


current  reference  tra 
1 327336 167398e+00 
-. 198312244C614O-01 


Jectory  state  vector 
-.57754C7550763e+00 
.3285241 134858e-0l 


at  epoch: 

•3068164374417e-00 

•336l064129534e-00 


-.46647773505760-02 

.43100292941420+01 


Last  Pass  Residuals: 


time,  res  » 
time,  ros  “ 
time,  ros  * 
time,  ros  * 
time,  ros  * 


.5207363090-01 
.5256940940-01 
.5306518790-01 
. 5356096650-01 
.5405674500-01 


.5607842280-05 

.5559693840-05 

.5509598150-05 

.5457346100-05 

.5404726390-05 


. 1952846670-06  - 
.1647327340-86  - 
.  1352404320-06  - 
.1101640710-06  - 
.3283131340-07  - 


237744529e-05 
20334307 le-05 
1708705260-05 
1 403106470-05 
1 116307370-05 


CONVERGENCE  ACHIEVE!). 

In  nom I n 1  a  Gaussian  trajectorum  referentfa 
declarium  est  estlmatta 


t 


9 


9 


9 


( 
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NEXT  BAYES  .OOP 
tepoch  »  time  3 

P'r3t  Pas3  Residuals: 


1 1  me . 
time, 
time. 
1 1  me , 
time. 


.  101651483e*00 
. 1021 4  7262  a*00 
. 102643040e*00 
. 103 13881 9e+0O 
. 103634S97e*00 


100673452e-04 
104727234e-04 
1089368 10e-04 
1 13308403e-04 
1  17323512e-04 


.332567295e-05 
. 430990151e-05 
. 5416a0948e-05 
. 6 6 220203 8e- 05 
.7941 1 3728e-05 


.1851679690-04 
.  24 1 225845e-04 
. 3032552 1 5e-04 
.3713331 72e-04 
•445512757e-04 


Iteration  1 
state  corrections 

.14089420359300-05  . 10 L 9466 1 1 8635e-05  -. 22644909 1 3225e-05 

. 569773234 5062e-03  -. 1 4760701 4 1 4G3e-02  . 1 468 1 27904633e-01 

current  reference  trajectory  state  vector  at  time  3: 
-.1330975613393e*00  - . 5 7904C4 1 45 1 7 1 e+00  . 3096544393475e*00 

-.41352443860060-01  . 861 9843067786e-01  . 35078769 1 9998e*00 


.78043498001530-03 

.50536026256400*00 


time,  res  -  . 101 65 1 483e+00  -.8236124110-05 
time,  res  -  . 102 1 47262e*00  -.3217678900-05 
time,  res  *  .1026430400*00  - . 8 1 99306 37e-05 
time,  res  »  . 103 1 388 1 9e*00  -  .  8 1 8 1 43254e-05 
time,  res  »  . 103634597e*00  - . 8 1 6348789e-05 


.7808649140-06 
.7791 75096e-06 
.796032783e-06 
.305333625e-06 
.3230147840-06 


-.94592508999230-02 

.38041610315780*01 

.966 10009 1 e-06 
.7685522750-06 
.  549569884e-06 
.3053836160-06 
.  35894 6033e-07 


I  terat i on  2 
state  corrections 

.73996073870700-05  -.15275313180600-05  - . 66997536S8038e-06  -.33363689924570-04 
.33225l2.J40308e-04  -.40154004238960-04  . 352 103 1 280592e-01  - . 25 153 1 7 1 666 10e*00 

current  reference  trajectory  state  vector  at  time  3: 

-.13308965173170*00  - . 5790499420484e+00  . 3096538 1 9072 l e-00  -.94926145898480-02 

-.41314223739660-01  .861582766.73620-01  .38599850480570*00  .35526293149170*01 


time,  res 
time,  res 
time,  ras 
time,  res 
time,  res 


101651483o*00 
102147262e*00 
,  1026430400*00 
1031388190*00 
,  103634597o*00 


1825804060-06 
,  1808728790-06 
.1786412340-06 
.  1762357490-06 
.1732106840-06 


. Id! 704736e-08 
■ . 3293027 15o-08 
-.7353371020-08 
-.21 1995642e-07 
-.356493463e-07 


.9625695670-07 
. 108306370e-06 
.  142274295e-06 
.1955032960-06 
.2690181630-06 


i  terat I  on  3 
state  corrections 

.  1735326751436e-06  -. 3962636388 1  1 9e-07  - . 2576 102303298e-08  -.23329152982750-05 
. 315024456C552O-05  - . 4974 l 73782535e-05  . 63394 103 1 4836e-02  -. 25330058 l 74 1 5e-01 

current  reference  trajectory  state  vector  at  time  3: 

-. I  330894 331 990e*00  -. 57904998 1 6747e*00  . 3096538 1 72960e-00  -.94949475051460-02 

-.41311073495 09 e-01  . 86 1 5330249934o-0 1  . 3928379 1 5 l 205e-00  . 3527299256743e*01 


time,  res  ■ 
time,  res  = 
time,  res  = 
time,  res  “ 
time,  res  * 


101651 483e+00 
102 1 47262e*00 
102643040e*00 
10313831 9e*00 
I03634597e*00 


1345991 70e-03 
1  385 1 3304e-08 
1 78554985e-08 
194957389e-08 
1783755 1 7e-08 


.  383802701e-03 
.  35733 1 602e-08 
.3609173630-08 
.  4250140240-09 
.  1491000330-08 


. 1 70003 3 3 3e -08 
. 107068482e-08 
. 27327528 le-08 
. 40834 294 9e- 08 
•618284950e-08 


Iteration  4 
state  corrections 

.24419975275490-08  - . 5033 l 37 1 29 709e-09  -.13321687058600-09  - . 2425232470404e-07 
.26745632796950-07  - . 3642365757036e-07  . 6535595590269e-04  - . 3034730539397e-04 

current  reference  trajectory  state  vector  at  time  3: 

- . 1 330894.3075700*00  -. 579049982 1 735e*00  . 3096538 1 7 1 628e-00  -.94949717574710-02 

- . 4131 104674941e-01  . 36 1 53266076 1 8e-0 1  . 39290377 10764e-00  . 35272C3909438e+01 


time, 
time, 
time, 
1 1  me . 
time. 


10165 1 483e*00 
1021 4  7262e*00 
1026430400*00 
103138319e*00 
103634597e*00 


.6305254100-09 
.  565372C94e-09 
•629O75565e-09 
.421454777e-09 
. 533530029e-09 


.3733846120-08 
■ .  3563591530-08 
.3825337710-08 
. 7923173230-10 
-.6109264210-09 


.8037331540-09 
.  4  1  3223567e-09 
.  1834  10289e-09 
.  1 0227  904  5e- 10 
.  649873827e- 10 
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1  ter  at  I  on  5 
state  corrections 

•  .2316313341378®- 11  - . 5 1 33 4G57 1 3550e- 1 2  2058 1 23034 1 34e- 1 2 

.8633814355239e~Il  . 4240745285 1 3 1 e- 1 1  -. 48601 737 45442e-09 


current  reference  trajectory  state  vector  at  time  3: 

1320894307542er00  -. 579049982 1 79 1 e+00  . 0035533 1 7 1 625e-00 

-.4131 104574077e-0 1  . 35 1 3 326608042e-0 1  . 3929037705904e-J0 


Last  Pass 


Res  1 dua 1 s : 


time,  res 
time,  res 
tine,  res 
tine,  res 
time,  res 


.  101651 483e+00 
. 1021 47262e*00 
. 102643040® +00 
. 1031 388 19e*00 
. 103634597e+00 


. 633384635e-09 
. 56821 3934e-09 
. S32699247e-09 
. 4242G01 33e-09 
.536316831 e-09 


. 3733557  4Ge-08 
356336831e-08  - 
,382507415e-08 
. 789824872e-10  - 
-.611 1 5990We-09 


CONVERGENCE  ACHIEVED. 

In  nominia  Gaussian  tra Jectorum  referentia 
declarium  est  estimatia 


< 


-.334  1 24  34  77990e-  10 
. 7  662  20205 9526 e-08 


- . 9494971 790884e-02 
. 3527263917 100e»0 l 


80369 1 522e-09 
4 1 327698 le-09 
1 33333540e-09 
103245433e- 1 0 
G48592831e-I0 


1 16 


c 


» 
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. 449934b- 05 
e80988*>O8 
861985*06 
. 009000*  *00 
000000**00 


SUMMARY  OF  ALL  CASES 

- 

Ve  (DC/TU) 

M 

• 

Exact 

Stage  I 

.3172982  55  e+00 

.447963755e+01 

Stage  II 

. 3  92  9044  80  e+00 

.35  27  26545  e  +  01 

1 

Data  Segments  =  16 

Point  #: 

Conver  ged 

on 

• 

iteratio 

a  #: 

16 

.317315725  e+00 

.447  93  8985  e+01 

2 

32 

.317193607  e+00 

.4481029996+01 

1 

48 

.317  36  8164  e+00 

.4478740526+01 

1 

► 

64 

.317439486e+00 

.447790352e+01 

1 

80 

.3173458046+00 

.447907412e+01 

1 

• 

96 

.31747  853  7  e+00 

.447760474e+01 

1 

112 

.317518937  e+00 

.447727873  e+01 

1 

128 

.317471044e+00 

.447786183  e+01 

1 

144 

.  3 17  56  23  90  e+00 

.447706513  e+01 

1 

< 

160 

. 3 173 53  960e+00 

.447912668e+01 

1 

- 

176 

.316971039e+00 

.448249374  e+01 

1 

• 

192 

.675400244e+00 

.2699680346+01 

4 

208 

.473677  6  28  e+00 

.311701023  e+01 

3 

224 

.392931745e+00 

.352711260e+01 

3 

240 

. 3  932  96229  e+00 

•352519154e+01 

2 

> 

Data  Segments  ==24 

*» '  • 

* 

i 

24 

. 3 1730  50 53  e+00 

.447954511 e+01 

2 

-v-t 

48 

.317304049  e+00 

.447  9556  37  e+01 

1 

72 

.317  2  83  474  e+00 

•447981709e+01 

1 

% ' 

96 

.3173538126+00 

.447900588e+01 

1 

• 

. . 

120 

.317308991 e+00 

.4479525  53  e+01 

1 

4 

144 

.3173929746+00 

.447871385  e+01 

1 

■* 

168 

.317  26  8608  e+00 

.447990303  e+01 

1 

192 

.361079891e+00 

.414022556  e+01 

3 

216 

.406305787  e+00 

.345281348  e  +  01 

4 

240 

.393116024e+00 

.352614137 e+01 

3 

• 

Data  Segments  =  32 

32 

.317308409e+00 

,447949574e+01 

2 

■y'.y 

64 

.317306635  e+00 

.447  9  53176  e  +  01 

1 

96 

.317305226e+00 

.447955992e+01 

1 

• 

128 

.317324984  e+00 

. 447  93  5  5  43  e  +  01 

1 

160 

.317285360e+00 

.447976097e+01 

1 

192 

.331250475 e+00 

.436375095e+01 

3 

224 

.3969134176+00 

.3  50494767  e+01 

4 

•  *.•  ’ 

256 

.3928415456+00 

.3527586166+01 

2 

9 

-  — . 
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v 

t 

-- 

oil 

Continued : 


Point  #: 


(DD/TU) 


Data  Segments  = 


Converged  on 
iteration  # 


,3173  03  338  e+00 
,  317296296 e+00 
,317  2  953  26  e+00 
.320304527  e+00 
.3  93636292  e+00 


.447  956672e+01 
.447966247  e+01 
.447  966670e+01 
.445333709  e+01 
.3  523  27  942  e+01 


Data  segments  = 


.  3 17  3041 10  e+00 
.317296361 e+00 
.317291091 e+00 
.6  86  866  862  e+00 
.392921506  e+00 


.447  95  5  5  85  e+01 
.447966253  e+01 
.447  97  093  3  e+01 
.261674361 e+01 
.352717312  e+01 


Data  Segments  = 


.3172999  85  e+00 
. 3 173006 26  e+00 
.3183316  99  e+00 
.  3  93 1 276 34  e+00 
.392 90 5747 e+00 


.447  961325  e+01 
.447  960  9  82  e+01 
.447028787 e+01 
. 3  5 260  807 4  e+01 
.352725914e+01 


Data  Segments  = 


.317298046 e+00 
. 3 173023  51 e+00 
.416586494e+00 
.392918156 e+00 
.392871250 e+00 


.  44796 4018e+01 
.  447  95  85  87  e  +  01 
.3  56484600e+01 
.352720315  e+01 
,352740128e+01 


Data  segments  =  66  ** 


.31730043  2e+00 
.317301 918e+00 
.364060396  e+00 
.3  92  912142  e+00 
.3  92902400e+00 


.447  96  07  2 5  e+01 
.447  959537  e  +  01 
.409734550e+01 
.3  5  27224  56  e  +  01 
.3527274406+01 


Data  Segments  =  100 


.317297143  e+00 
.336106412e+00 
.3929037706+00 


.447  965233  e+01 
.  43 1002929  e+01 
.352726891e+01 


Values  of  .9  .9  .9  .9  .9  .9  «7  .7 
Values  of  .8  .8  .8  .8  .8  .8  .5  *5 
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The  estimation  of  launch  vehicle  performance 
parameters  was  explored  through  the  use  of  a  uayes 
Filter.  The  main  emphasis  was  to  use  an  eight  state 
model  that  would  include  the  vehicle  position  and 
velocity  vectors,  the  vehicle  exhaust  velocity,  and 
the  ratio  of  the  mass  flow  rate  to  the  initial  mass. 

A  primary  objective  was  to  be  able  to  observe  these 
quantities  through  the  staging  events,  where  the  last 
two  elements  would  be  changing  very  rapidly.  The  results 
indicated  that  indeed;  the  staging  event  was  observable, 
however,  as  would  be  expected,  the  data  processed  at 
the  exact  time  of  staging  included  errors  which  dim¬ 
inished  as  the  filter  processed  more  data.  A  fading 
memory  was  added  in  an  attempt;  to  improve  the  filters 
performance  in  the  area  of  a  staging  event.  This  proved 
to  be  marginally  successful  as  several  Bayes  loop 
iterations  had  to  be  performed  to  notice  the  effect 
of  the  fading  memory  addition.  Gare  was  taken  to 
show  each  step  of  the  filter  development  and  its 
checkout.  Several  numerical  tables  are  presented 
including  the  input  and  output  data. 
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